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PROGRAM  LISTINGS 


I" 


Fi  ' ■ 

K:  ■ £■ 


:k:' 


ooonoo 


PSOGRAH  XNSSEO  (INPUT, OUTPUT, TAPEO,TAPE6=OUTPUT,TAPE7,TAPE10, 

1 TAPE12,TAPE20,TAPE30,TAPEB0,TAPE50,TAPE60,TAPE6S,TAPE67,TAPE69, 

2 TAPE 70, TAPE80,TAPE90, TAPES) 

03/10/78  OATE  OF  CURRENT  MODULE 


THE  SEQUENCER  MODULE,  THE  EXECUTIVE,  19  THE  HAXN  PROGRAM, 


00000010 
00000020 
00000030 
00000090 
00000050 
00000060 
WHICH  CALLS00000070 


■ OTHER  MODULES 

00000080 

00000090 

PEAL 

AB0( 3) 

00000100 

REAL 

A0I3) 

00000110 

BEAL 

ALT 

00000120 

PcAL 

ALTO 

00000130 

REAL 

BUF1I3) 

000001*0 

REAL 

BUF2C3) 

00000150 

REAL 

3UF319) 

00000160 

PEAL 

DATA! 9) 

00000170 

REAL 

0THETA13J 

00000180 

REAL 

DTHET0C3I 

00000190 

PEAL 

OTHETZO) 

00000200 

REAL 

OT 

00000210 

PEAL 

OTSLOH 

00000220 

REAL 

OVt  3 ) 

00000230 

REAL 

0VA(3 ) 

000002*0 

REAL 

OVN(  3) 

QOOQOC50 

REAL 

0V013) 

00000260 

PEAL 

OVT(  3) 

00000270 

REAL 

FT 

00000260 

REAL 

G 

00000290 

REAL 

Hoifa 

00000300 

PEAL 

LAT 

00000310 

REAL 

LON 

00000320 

REAL 

MOOPDT 

00000330 

PEAL 

NAVH 

000003*0 

REAL 

NAVLAT 

00000350 

REAL 

NAVLON 

00000340 

PCAL 

NAVVt  31 

, 00000370 

REAL 

OUTPSM 

00000380 

REAL 

POUF (161 

00000390 

REAL 

P0ATA(20> 

00000*00 

REAL 

PITCH 

00000*10 

REAL 

PSNTOT 

00000*20 

REAL 

PftNTSH 

00000*30 

REAL 

RE 

00000**0 

REAL 

ROLL 

00000*50 

REAL 

SPAREl 

00000*60 

REAL 

SPARES 

00000*70 

REAL 

SPARE3 

00000*80 

REAL 

T 

00000*90 

REAL 

TEND 

00000500 

PEAL 

T PR  MOD 

00000510 

REAL 

TPflNT 

00000520 

REAL 

TSEQ 

00000530 

REAL 

VELD) 

000005*0 

REAL 

WOE  31 

00000550 

MEAL 

«0B(3! 

00000560 

REAL 

REBOOT*  31 

00000670 

REAL 

WE 

00000580 

REAL 

WONDER 

00000590 

REAL 

XI 

00000600 
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c 


c 


c 


c 


c 


REAL  X2 
REAL  X3 
REAL  X4 
REAL  YAW 

REAL  0CCI9I 
REAL  OCM( 9) 
REAL  TESTIOt  9) 
"EAL  NAVP 
REAL  NAVR 
REAL  NAVHD 

INTEGER  I 
INTEGER  IEN0F 
INTEGER  IFI 
INTEGER  IFILE 
INTEGER  OFILE 
INTEGER  PFILE 
INTEGER  3IMEN0 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 


(OATA(l)  iOT) 

I DATA!  2)  iPRNTSH) 
( OATAt  1)  .OyTPSM) 
(OATAKi)  tXFILE ) 

I DATA1 5 1 • SPARE 1 1 
(OATAI6 > .SPARES! 
(DATA! 7 J .TEND I 
IDATAIfll  .OTSLOU) 
(DATA) 9 > .NQOPOT) 


EQUIVALENCE  IPOATAI1!,  ME  I 
EQUIVALENCE  (P0ATAI2).  RE  I 
EQUIVALENCE  (POATAO),  G» 
EQUIVALENCE  IP0ATA(4I.  PffNTOT) 
EQUIVALENCE  «FOATA(&),  PfiUPUU 


DATA  ACB/3*0.0/ 
dATA  A0/5»0.0/ 
DATA  ALT/O.O/ 

DATA  ALTO/O.O/ 
DATA  OATA/9»O.0/ 
dATA  dTHETA/3<*0.0/ 
OAT*  OYHETO/5*,9.0/ 
DATA  BTIietE/>0.0/ 
DATA  OV/3«Q.O/ 

DATA  OVA/3*0.O/ 
dATA  OW3*C.O/ 
DATA  OV0/3*O.O/ 
DATA  OVT/3«0.O/ 
CATA  FY/9.0/ 

DATA  U170.0/ 

DATA  LOH/O.O/ 

DATA  HAVH/O . 0/ 
DATA  NAVLAT/O.O/ 
DATA  HAVLO.VO.O/ 
OATA  HAW/ S'.  0.0/ 
DATA  POATA/C0*0.0/ 
OATA  PITCH/0.0/ 
OATA  ROLL/O.O/ 
DATA  T/0.0/ 


00000610 

00000620 

00000630 

00000690 

00000650 

00000660 

00000670 

00000680 

00000690 

00000700 

0000Q710 

00000720 

00000730 

00000740 

00000750 

00000760 

00000770 

00000760 

00000790 

00000800 

00000810 

00000820 

00000330 

00000840 

00000850 

00000060 

00000870 

00000800 

00000890 

00000900 

00000910 

00000920 

00000930 

00000940 

00000950 

00000960 

00000970 

00000980 

00000990 

00901000 

00801010 

00001020 

00001030 

00001040 

00001050 

00001060 

00001070 

00001030 

00001090 

00001100 

ooooino 

00001120 

00001130 

60001140 

00001150 

00001160 

00001170 

ooooueo 

00001190 

00001200 
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TPRNT*TPRNT*PRNTOT 

00001810 

c 

00001820 

8 CONTINUE 

00001830 

IF(  PRNTSW.  LT . 1 . ) GO  TO  4 

00001840 

WRITE  ( OFILE 1 506  ) TESTID 

00001850 

WRITE  (OFILE. 504!  T 

00001860 

c 

00001870 

4 CONTINUE 

000016E0 

c 

00001893 

c 

00001900 

c 

00001910 

c- 

"•TRAJECTORY  MODULE*** 

00001920 

c 

00001930 

c 

INPUT:  TIME 

00001940 

c 

T 

00001950 

c 

00001960 

c 

OUTPUT:  LATITUDE. LONGITUDE. ALTITUDE. VELOCITY, DELTA  VELOCITY, 

00001970 

c 

PITCH, YAW, ROLL, HEADING, WANDER, BODY  ACCEL, BODY  RATES, 

00001980 

c 

INERTIAL  TO  BODY  MATRIX 

00001990 

c 

LAT, LON, ALT, VEL, PITCH, YAW, ROLL, DVT, HDING, WANDER, AB.WB 

C0002000 

c 

00002010 

XI  = T 

00002020 

IF1=IENDF 

00002030 

EUFll 1 )= PRNTSW 

00002040 

6UF1(2)=MOOPOT 

00002050 

c 

00002060 

CALL  TRAJ  ( XI,  IENOF , PRNTSW,  MODPDT , 

00002070 

X LAT, LON, ALT, VEL, PITCH, YAW, ROLL, DVT, HDING, WONDER, AB,UB) 

00002080 

c 

00002090 

IF  (IENOF  .NE.  0)  SIMEND  = 1 

00002100 

• 

c 

00002110 

c 

00002120 

c 

00002139 

C*»* ENVIRONMENT  MODULE*** 

00002140 

- 

C 

00002150 

c 

INPUT:  TIME, GRAVITY  WRT  BODY,  BODY  RATES, INERTIAL  TO  BODY  MAT 

00002160 

c 

T, AB.WB 

00002170 

c 

00002180 

c 

OUTPUT:  INPUT  VALUES  WITH  VIBRATION,  BODY  ACCELATION.QBI  ♦ VIBRATIQN00002190 

c 

ASB.WSB.WBBDOT 

00002200 

c 

00002210 

XI  = T 

00002220 

IF1  = IENOF 

00002230 

c 

00002240 

DO  10  1=1,3 

00002250 

BUF1(H=AB(I) 

00902260 

BUF2I I )=W3( I ) 

00002270 

10  CONTINUE 

00002280 

C 

00002290 

C 

00002300 

CALL  ENV(X1,IF1,BUF1,BUF2, 

00002310 

X ABB,WB3,WBBC0T) 

00002320 

C 

00002330 

IP  (IF1  .NE.  0)  SIMEN0=1 

C0002340 

c 

00002350 

c 

00002360 

•c 

00002370 

C*»*GYRO  MODULE*** 

00002380 

C 

OPERATES  ON  .001  SECOND  CYCLE 

00002390 

C 

00002400 

- 

1-5 

c 

INPUT:  TIME, BODY  RATES, CHANGE  IN 

BOOT  RATES,  BOOY 

ACCELATION 

00002410 

c 

T.IOB.WBBOOT.ABB 

00002420 

c 

00002430 

c 

OUTPUT-'  GYRO  PUPTIBATIONS 

00002440 

c 

OTNETA 

00002450 

c 

00002460 

XI  =T 

00002470 

IF1=IENDF 

000024&0 

DO  CO  1=1,3 

00002490 

BUFK  I )=k'BB<  I ) 

00002500 

CUrC(I)=WEBOOTlI) 

00202510 

6l'F3tI)=AB3(I) 

00002520 

20 

CONTINUE 

00002530 

C 

00002540 

CALL  GYROS  (XI,  IF1,  BUF1,  BUF2, 

, BUF3, 

00002550 

X D THETA ) 

00002560 

IF  ( IF1  .NE.  0)  SIHENO=l 

00002570 

C 

00002500 

c 

00002590 

c 

00002600 

C*»#ACCELEROMET£R  MODULE##** 

00002610 

C 

00002620 

C 

INPUT:  time, BODY  accelation.body 

RATES, CHANGE  IN 

BODY  RATE 

00002630 

C 

T.ABD.WBB.UBBDOT 

00002640 

c 

00002650 

c 

OUTPUT:  DELTA  VELOCITY 

00002660 

c 

DV 

00002670 

c 

00002680 

X1=T 

00002690 

IF1=IEN0F 

00002700 

DO  30  1=1.3 

00002710 

BUFK  I )=ABB(  I ) 

C0002720 

EUF2I I )=WBB( I ) 

00002730 

B'JF3(  I ) =WBBDOT(  I ) 

00002740 

30 

CONTINUE 

00002750 

C 

00002760 

CALL  ACCEL  ( XI , IF1 ,BUF1 ,DUF2 ,BUF3, 

00002770 

X OV) 

00002780 

C 

00002790 

IF  (IF1  .NE.  0)  SJMENO=l 

oococeoo 

C 

00002310 

C 

00002820 

c 

END  OF  SEQUENCER  FAST  CYCLE 

00002830 

c 

00002840 

c 

IF  SIMULATION  LAST  PASS  (IENDF=1) 

DONT  CYCLE  FAST 

CYCLE, 

00002350 

c 

BUT  FALL  INTO  SLOW  CYCLE  AND  TERMINATE 

00002860 

c 

00002670 

. IF  (IEN0F.EQ.1)  GO  TO  39 

00002680 

*c 

' 

00002890 

IF  ( T. LT.TSEQ- .0005)  GO  TO  72 

00002900 

TSEQ*TSEQ*OTSLOW 

00002910 

c 

00002920 

c 

00002930 

39  CONTINUE 

00002940 

c 

00002950 

c 

THE  FOLLOWING  MODULES  OPERATE  ON 

THE  SLOW  CYCLE 

00002960 

!c 

00002970 

c 

00002980 

c 

00002990 

C# “"ALTIMETER  MODULE### 

00003000 
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INPUT:  TIME,  TRUE  ALTITUOE,  VELOCITY 

t.alt.vel 

OUTPUT:  ALTITUOE 
ALTO 


XI=T 

IF1=IEN0F 
00  40  1=1,3 
BUF1(  1 )=VEL(  I ) 

40  CONTINUE 

buf:(i»=alt 

CALL  ALTI  <X1,IF1,6UF2(1),BUF1, 
X ALTO) 

IF  ( IF1  .NE.  0)  SIMEN0=1 


"REAOER  MOOULE*"" 

INPUT:  TIME,  GYRO  PURTIBATIONS,  DELTA  VELOCITY 
T,0THETA,0V 

THE  REAOCR  INPUT  ITEMS  OTKETA  AND  DV  ARE  OUTPUT  AS 
ZERO  QUANTITIES 


OUTPUT:  GIMB4L  ANGLES,  OELTA  VELOCITY 
OTHETO.OVO 


X1=T 

IF1=IENDF 

CALL  R0R< XI , IF1 ,DTHETA,0V • 
X OTHETO.OVO) 

IF  (IFX.NE.O)  SIMENO=l 


"••ACCELEROMETER  COMPENSATION  MODULE*"" 


INPUT:  TIME,  DELTA  VELOCITY 
T.OVO.OTKETO 

OUTPUT:  COMPENSATED  DELTA  VELOCITY 
OVA 


XI=T 

Ir 1=IENDF 
00  45  1=1,3 
BUFK  I )=DVO(  I ) 
BUF2( I )=0THET0( I ) 
45  CONTINUE 


00003010 

00003020 

00003030 

00003040 

00003050 

00003060 

00003070 

00003000 

00003090 

00003100 

00003110 

00003120 

00003130 

00003140 

00003150 

00003160 

00003170 

00003160 

00003190 

00003200 

00003210 

00003220 

00003230 

00003240 

00003250 

00003260 

00003270 

00003260 

00003290 

00003300 

00003310 

00003320 

00003330 

00003340 

00003350 

00003360 

00003370 

00003380 

00003390 

00003400 

00003410 

00003420 

00003430 

00003440 

00003450 

00003460 

00003470 

00003480 

00003490 

0000350C 

00003510 

00003520 

00003530 

00003540 

00003550 

00003560 

00003570 

00003560 

00003590 

00003600 
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c 

CALL  ACOMPI XI , IF1 ,BUF1 ,BUF2 , 

X OVA) 

C 

IF  (IFI.NE.O)  SIMENO=l 
C 
C 
C 

C*»*GYRO  COMPENSATION  MODULE*'** 

C 

C INPUT:  TIME.GIMBAL  ANGLES, COMPENSATED  DELTA  VELOCITY 
C T.DTHETO.OVO 

C 

C OUTPUT:  COMPENSATED  GIMBAL  ANGLES 
C DTHETZ 

C 

XI  =T 

IF1=IENDF 

C 

DO  47  1=1,3 
EUF1( I )=DTHETO( I ) 

BUF2( I )=OVA( I ) 

47  CONTINUE 
C 

CALL  GCOMPI XI . IF1 ,BUF1 ,BUF2 , 

X DTHETZ ) 

C 

IF  (IFI.NE.O)  SIMEND=1 
C 
c 
c 

C***ALCOfJITHUM  MOOULE*** 

C 

C INPUT:  TIME. COMPENSATED  GIMBAL  ANGLES,  COMP.  DEL  VELOCITY 
C T, DTHETZ, DVA 

C 

C OUTPUT:  DELTA  VELOCITY,  DIRECTION  COSINE  MATRIX 
C DVN.DCM 

C 

X1=T 

IF1=IENDF 
DO  50  1=1,3 
BUF1( I )=DTHETZ( I ) 

EUF2I I )=DVA( I ) 

50  CONTINUE 
C 

CALL  ALG  (X1.IF1.BUF1.BUF2, 

X DVN.DCM) 

'c 

IF  ( IF1  .NE.  0)  SIMENO=l 
C 
C 
C 

C***NAVIGATION  MODULE*** 

C 

C INFUT:  TIME,  OELTA  VELOCITY,  ALTITUDE, DIRECTION  COS  MATRIX 
X T ,DVN,ALTO,DCM 

C 

C OUTFUT:  LATITUDE, LCN5ITU0E, VELOCITY, ALTITUDE, LV  RATES, ATTITUDE 
C NAVL AT , NAVLCN , NAW , NA VH , NAVP , NAVR , NAVHD 


00003610 

00003620 

00003630 

00003640 

00003650 

00003660 

00003670 

00003680 

00003690 

00003700 

00003710 

00003720 

00003730 

00003740 

00003750 

00003760 

00003770 

00003780 

0C003790 

00003800 

00003810 

00003620 

00003830 

00003840 

00003850 

00003860 

00003870 

00003SS0 

00003890 

00003900 

00003910 

00003920 

00003930 

00003940 

00003950 

00003960 

00003970 

00003980 

00003990 

00004000 

00004010 

00004020 

00004030 

00004040 

00004050 

00004060 

00004070 

00004030 

00004090 

00004100 

00004110 

00004120 

00004130 

00004140 

00004150 

00004160 

00004170 

00004180 

00004190 

00004200 
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C 

X1=T 

IF1=IEN0F 

X2=ALT0 

C 

CALL  LLN  (X1,IF1,0VN,X2,0CM, 

X NAV  L AT . N AV  LON , N AVV , NAVH , N AVP , NAVR , NAVHO ) 

C 

IF  (IF1  .NE.  0)  SIMEND=1 


••EVALUATION  MODULE*** 

COMPUTES  AND  PRINTS  TABLE  DATA  EVERY  50  CYCLES 

INPUT:  TIME, TRAJECTORY, NAVIGATION, AND  ATTTUDE  PARAMETERS 

T.LAT, LON, ALT, VEL, DVT, PITCH, ROLL, YAM, WONDER, NAVLAT,NAVLON, 
NAVV , NAVH , NAVP , NAVR , NAVHD 

OUTPUT:  NOTHING 


XI=T 

IF1=IENDF 

CALL  EVL  (XI, IFl.LAT, LON, ALT, VEL. DVT, PITCH, ROLL, YAM, WONDER, 
XN AV  LAT , NAV  LON , NAVV , NA VH , NAVP , NAVR , NAVHD ) 

IF  ( IFI.NE.O  ) SIMENO  = l 


72  CONTINUE 
C 

IF  ( IENDF  .EG.  1)  GO  TO  1000 
IF  (T  .GE.  TEND-. 0005)  GO  TO  81 
IF  (SIMEND  .NE.  0)  GO  TO  61 
IF  (T  . EQ.  0)  MRITE(OFILE ,501 ) 

C 

T=T+DT 

IT=T 

FT=T-IT 

IF  (FT  .LT.  .999)  GO  TO  80 
T=IT*1.0 
80  CONTINUE 
C 

GO  TO  3 
C 

61  CONTINUE 
C 

C SIMULATION  END  TIME  REACHED 

C 

IENDF=1 

C 

GO  TO  3 
C 

1000  CONTINUE 

PRINT  502,  T 
1 STOP 
C 

500  FORMAT  (30H  SEQUENCER  INITIALIZATION 
X /3X.8H  DT  , 3X.G16 . 8, 3X, AH  SEC, 


00009210 

00009220 

00009230 

00009290 

00009250 

00009260 

00009270 

00009280 

00009290 

00009300 

00009310 

00009320 

0C009330 

00009390 

00009350 

00009360 

00009370 

00009380 

00009390 

000C9900 

00009910 

00009920 

00009930 

00009990 

00009950 

00009960 

00009970 

00009980 

00009990 

00009500 

00009510 

00009520 

00009530 

00009590 

00009550 

00009560 

00009570 

00009580 

00009590 

00009600 

00009610 

00009620 

00009630 

00009690 

00009650 

00009660 

00009670 

00009600 

00009690 

00009700 

00009710 

00009720 

00009730 

00009790 

000C9750 

00009760 

00009770 

00009780 

00009790 

00009800 
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X /3X.8H  FRNTSW  .3X.G16.8, 

X /3X,8H  OUTPSW  ,3X,G16.8, 

X /3X.8H  OFILE  ,3X,I13, 

X /3X.SH  PRNTOT  ,3X,G16.8, 

X /3X.OH  M03PDT  ,3X,G16.8, 

X /3X.8H  TEND  ,3X,G16.3, 

X /3X.8H  OTSLOW  ,3X,G16.8,/) 

501  FORMAT  C1H1.25H  ***START  SIMULATION****  ,//  ) 

500  FORMAT  (//26H  ****SIMl'- ATICN  COMPLETE  AT  ,G16.8,  AH  SEC  ) 

503  FORMAT  (I5.F20.10) 

504  FORMAT  (5X.13H  **#  SEQ  ****  TIME=  .G16.8,//) 

505  FORMAT  ( 30H  PHYSICAL  DATA  FILE  , 

X/3X.8H  WE  ,3X,G16.8,3X,8H  RAO/SEC, 

X/3X.CH  RE  ,3X,G16.8,3X,8H  FT  , 

X/3X.3H  G ,3X.G16.8,3X,8H  FT/SEC2, 

X/3X.8H  PRNTOT  , 3X ,G16 .8 , 3X.6H  SECS  ,///) 

506  FORMAT  (1H1.9A8,/) 

507  TOKMAT  ( 9A8 ) 

600  FORMAT  (12X.I15) 

END 


00004810 

00004820 

00004830 

00004840 

00004G50 

00004860 

00004870 

00004880 

00004890 

00004900 

00004910 

00004920 

00004930 

00004940 

00004950 

00004960 

00004970 

00004980 

00004990 

00005000 

00005010 
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03/10/78  OATE  OF  CURRENT  MODULE 


000000X0 
00000020 
00000030 

THE  INTERIM  TRAJECTORY  MODULE  INTEGRATES  THE  STANDARD  LOCAL  00000040 
LEVEL  NORTH-SLAVED  NAVIGATION  EQUATIONS  USING  THE  WGS72  ELLIPSOIDAOOOOOOSO 
EARTH  MODEL  AND  FIRST  ORDER  ALGORITHMS  THROUGHOUT,  AS  SUCH  IT  00000060 
CONTAINS  FHASING  ERRORS  WHICH  WILL  CAUSE  HIGHER  ORDER  ALGORITHMS  00000070 
TO  HAVE  APPARENT  ERRORS.  NOTE  THAT  INITIAL  VELOCITIES  ARE  INPUT 
IN  AN  ENU-NORTHSLAVEO  FRAME. 


SUBROUTINE  TRAJ  <T,IENDF.TBUF1,TBUF2, 

X LAT, LON, ALT, VEL, PITCH, YAW, ROLL, DV.HDING. WANDER, AB.WB) 


REAL 

ABO) 

REAL 

ADIO) 

REAL 

A6PO) 

REAL 

ALT 

REAL 

TBUF1 

REAL 

TBUF2 

REAL 

COSLN 

REAL 

COSLT 

REAL 

DATAI 15) 

REAL 

DELON 

PEAL 

DT 

REAL 

DVO) 

REAL 

ESQ 

REAL 

FOHSO) 

REAL 

GR 

REAL 

GN 

REAL 

G 

REAL 

GVECO) 

REAL 

HDING 

REAL 

IALT 

REAL 

I LAT 

REAL 

I LON 

REAL 

INTLON 

REAL 

IPITCH 

REAL 

IROLL 

REAL 

ITEMP( 9 ) 

REAL 

IVELO) 

REAL 

I YAW 

REAL 

LAT 

REAL 

LON 

REAL 

MODPDT 

REAL 

MTEMP( 9) 

REAL 

PBUF<16) 

REAL 

PDATA( 20 

REAL 

PDOT 

REAL 

PITCH 

REAL 

PRNTOT 

REAL 

QBK  9 ) 

REAL 

QIBOLDt 9 

REAL 

GBP<9) 

REAL 

QEK  9) 

REAL 

QPE(  9 ) 

REAL 

QCP( 9 ) 

REAL 

QTEMP( 9) 

REAL 

RM 

REAL 

RP 

REAL 

ROOT 

00030080 

00000090 

00000100 

00000110 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

00000180 

00000190 

00000200 

00000210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 

00000280 

00000290 

00000300 

00000310 

00000320 

00000330 

00000340 

00000350 

00000360 

00000370 

00000380 

00000390 

00000400 

00000410 

00000420 

00000430 

00000440 

00000450 

00000460 

00000470 

00C00480 

00000490 

00000500 

00000510 

00000520 

00000530 

00000540 

00000550 

00000560 

00000570 

00000580 

00000590 

00000600 
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REAL 

RDTODG 

REAL 

ROLL 

REAL 

RE 

REAL 

SINLN 

REAL 

SINLT 

REAL 

SPARE1 

REAL 

T 

REAL 

TPRMOQ 

REAL 

TPRNT 

REAL 

TTRJ 

REAL 

VELGUX3) 

REAL 

VEL<3) 

REAL 

W<3) 

REAL 

ME 

REAL 

WB(3) 

REAL 

WBP(  3) 

REAL 

KoDOTI  3) 

REAL 

UBOLDt  3) 

REAL 

WANDER 

REAL 

WTEMP19) 

REAL 

YAM 

REAL 

YOOT 

REAL 

XWANDR 

REAL 

CWET 

REAL 

SWET 

REAL 

CP 

REAL 

SP 

REAL 

CR 

REAL 

SR 

REAL 

CY 

REAL 

SY 

REAL 

SPSR 

REAL 

SPCR 

REAL 

SALF 

REAL 

CALF 

REAL 

QOC( 9) 

INTEGER  IENDF 
INTEGER  XNIT3M 
INTEGER  OUTSW 
INTEGER  IFIIE 
INTEGER  PPILE 
INTEGER  OFILE 

EQUIVALENCE  (DATA(i)i  OT) 
EQUIVALENCE  (DATA<2),  PRNTSMI 
EQUIVALENCE  (OATAC3),  OUTSMJ 
EQUIVALENCE  (OATA(A)i  JCFXL8) 
EQUIVALENCE  IDATA(S).  ILATI 
EQUIVALENCE  )DATA(6)*  ILOH) 
EQUIVALENCE  tOATA<7)»  I ALT) 
EQUIVALENCE  I DATA) 6) » XVELU)) 
EQUIVALENCE  (DATA(R)i  XVEM2)) 
EQUIVALENCE  (0ATA(10)»  1VEM3U 
EQUIVALENCE  (OATA(H)i  IPITCK) 
EQUIVALENCE  (DATA) 12 >t  XYAM) 
EQUIVALENCE  (DATA(13)i  XROLU 
EQUIVALENCE  (DATA! 14 )»  HOOPOT) 
EQUIVALENCE  (0ATAU5).  INANQR) 


00000610 

00000620 

00000630 

00000640 

00000650 

00000660 

00000670 

00000660 

00000690 

00000700 

00000710 

00000720 

00000730 

00000740 

00000750 

00000760 

00000770 

00000700 

00000790 

00000000 

OOOOOOIO 

00000020 

00000030 

00000090 

00000050 

00000060 

00000670 

OOOQOOOO 

00000090 

00000900 

00000910 

00000920 

00000930 

00000940 

00000950 

00000960 

00000970 

00000900 

00000990 

00001000 

00001010 

00001020 

00001030 

00001040 

00001050 

00001060 

00001070 

00001060 

00001090 

00001100 

00001110 

00001120 

00001130 

00001140 

00001150 

00001160 

00001170 

00001160 

00001190 

00001200 
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C 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 


(PDATAtl), 
( P0ATA1 2 ) , 
( PDATA( 3 ) , 
( PDATA<  4 ) , 
< P0ATA( 5 ) . 


ME) 

RE) 

G> 

PRNTOT) 

PBUF(l)) 


DATA  ESQ/. 006694317778/ 
DATA  RDTODG/57. 29577951/ 
OATA  TFRMOD/O . 0/ 

DATA  TPRNT/0 . 0/ 

DATA  INITSW/0/ 

DATA  IFILE/20/ 

OATA  PFILE/7/ 

IF  (IENDF.EQ.1)  RETURN 
IF  (INITSM.EQ.O)  GO  TO  500 
IF  ( T. LT.TTRJ-.0005  ) RETURN 

DO  110  1=1,3 
VELOLDt I )=VEL( I ) 

110  CONTINUE 


ALT=ALT*VEL(3)«0T 

GR  = -<  32 . 0877057* . 16939081«SINLT*SINLT*7. 52610E-4* 

X SINLT**4 )*(  1 . - ( 9.6227E-8  -6.4089E-10*SINLT*SINLT)« 

X ALT+6.8512E-15*ALT«ALT) 

GN=1 .63E-8*ALT*SINLT*C0SLT 

DV<3)=-(VEL<l)*(W(2)*2.*WE«COSLT> 

X -VEL(  2 )*«WC  1 )*GR  )*DT*VEU  3 )-VELOLO(  3) 

DV(  2 ) = -(  VEL<  3 l*W(  1 )-VEL(  1 )*(  W(  3) 

X *2 . *WE*SINLT ) *GN )*DT*VEL( 2 )-VELOLD( 2 ) 

0V(  1 ) = -(  ( VEL<  2 )*<  Ml  3 ) + 2 . *KE»SINLT ) ) 

X -VEL(  3)*( HI  2 )*2.*WE*COSLT) )«DT*VELC 1 >-VELOLD( 1 ) 

COMPUTE  ACTING  FORCES  IN  NAVIGATION  COORDINATES  C LOCAL  VERTICAL) 

DO  120  1=1,3 
FORS( I )=DV( I )/DT 
120  CONTINUE 

RM=RE*(l.-ESQ)/(  <1.-ESQ*SINLT*SINLT>**1.5)  ♦ ALT 
RP=RE/SQRT( ( 1 .-ESQ#SINLT»SINLT) ) ♦ ALT 


U( 1 )=-VEL( 2 )/RM 
W(  2 )=VEL(  1 )/RP 
L AT=  LAT -W ( 1 ) #DT 
SINLT=SIN( LAT ) 

COSLT=COS( LAT) 

M( 3)=M( 2 )«SINLT/COSLT 
HANDER  = WANDER  - H(3)*DT 
SALF=SIN( WANDER) 

CALF=COS( HANDER) 
LON=LON*H( 2 )*0T/C0SLT 
SINLN=SIN( LON) 


00001210 

00001220 

00001230 

00001240 

00001250 

00001260 

00001270 

00001280 

00001290 

00001300 

00001310 

00001320 

00001330 

00001340 

00001350 

00001360 

00001370 

00001380 

00001390 

00001400 

00001410 

00001420 

00001430 

00001440 

00001450 

00001460 

00001470 

00001480 

00001490 

00001500 

00001510 

00001520 

00001530 

00001540 

00001550 

00001560 

00001570 

00001580 

00001590 

00001600 

00001610 

00001620 

00001630 

00001640 

00001650 

00001660 

00001670 

00001680 

00001690 

00001700 

00001710 

00001720 

00001730 

00001740 

00001750 

00001760 

00001770 

00001780 

00001790 

00001800 
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COSLN=COS( LON) 

00001810 

YDOT=-W<  3) 

00001820 

YAH  = YAW  ♦ YDOT*OT 

00001830 

c 

00001840 

c 

STORE  TRANSFORM  OF  QBI  COMPUTED  AT  T-DT 

00001850 

c 

00001860 

QIBOLDt 1 )=QBI( 1 ) 

00001870 

OIDOLO( 2 )=Q9I<  4 ) 

00001880 

QIBCLD( 3 )=CBI( 7 ) 

00001890 

QIB0L0(4)=QBI<2) 

00001900 

QID0LD(5)=QBI<5) 

00001910 

QXB0L0( 6 )=QBI( 8 ) 

00001920 

QIBOLDt  7 )=QSI( 3 ) 

00001930 

GIB0L0<8>=QSI<6> 

00001940 

QIBCLD(9)=QBI<9) 

00001950 

c 

00001960 

c 

STORE  BODY  RATES  COMPUTED 

AT  T-DT 

00001970 

c 

00001980 

DO  130  1=1,3 

00001990 

WB0l0m=WB(I) 

00002009 

130  CONTINUE 

00002010 

c 

00002020 

c 

GO  COMPUTE  INERTIAL  TO  BODY  TRANSFORMATION  (QBI) 

00002030 

c 

C0002040 

GO  TO  600 

00002050 

200  CONTINUE 

00002060 

c 

00002070 

c 

00002080 

c 

COMPUTE  BOOY  RATES  AND  ACCELERATIONS  IN  BODY  COORDINATES 

00002090 

c 

00002100 

CALL  MXM( QBI ,QIBOLD  >MTEMP) 

00002110 

c 

00002120 

W8(  1 )=MTEMP(  6 )/DT 

00002130 

H3( 2 )=-MTEMP( 3 )/DT 

00002140 

W313)=MTEMP(2)/0T 

00002150 

c 

00002160 

CALL  MXV( QBP .FOPS, AB ) 

00002170 

c 

00002180 

DO  220  1=1,3 

00002190 

WBDOTt I ) = ( WB( I )-WBOLO( I ) )/DT 

000C2200 

220  CONTINUE 

00002210 

c 

00002220 

c 

00002230 

ITEMPI 1 )=LAT*RDTODG 

00002240 

ITEMPI 2 ) = LON*RDTODG 

00002250 

ITEMPt  3 )=WANOER*ROTODG 

000 J2260 

c 

00002270 

c 

00002280 

'c 

OUTPUT  AND  PRINT  CONTROL 

00002290 

c 

00002300 

IF  (PRNTDT.GT.O)  GO  TO 

960 

00002310 

IF  (MODPDT.EQ.O)  GO  TO 

999 

00002320 

c 

00002330 

IF  (T. LT .TPRMOD- . 0005 ) 

GO  TO  999 

00002340 

TPRNT=TPRMOO<MODPDT 

00002350 

GO  TO  970 

000023o0 

‘.c 

00002370 

960  CONTINUE 

00002380 

IF  t T . LT . TPRNT- . 0005 ) 

GO  TO  999 

00002390 

TPRNT=TPRNT+PRNTOT 

00002400 
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970  CONTINUE 

IF  ( PRNTSW.LT. 1. ) GO  TO  999 

WRITE! OFILE, 1200)  AB,WB,mNP(l),ITENP(2>iITEMP(3)»ALT,VEL,DV 
USITEl OFILE  > 1 J00 )QBI 


999  CONTINUE 
TTRJ=T*0T 
RETURN 

INITIALIZATION  BEGINS  HERE 

500  CONTINUE 
REWIND  IFILE 
REWIND  PFILE 

501  REAO  (IFILE. 1000)  IX.DATA(IX) 
IP  (EOF(IFILE))  502.501 

502  CONTINUE 

503  REAO  (PFILE. 1000)  IX.POATA(IX) 
IF  (EOF(PFILE) ) 510.503 

510  CONTINUE 
REWIND  IFILE 
REWIND  PFILE 

OFILE=XPILE 


LAT=ILAT/R0TQ0G 
LONslLON/'ROTOOG 
INTL0HSL0N 
SINLT-SIN(  LAT) 

C0SLTcC05(  LAT  I 
SINLN=SIN(  LON) 

COSLN:C03(LON) 

ALT*XALT 

RH=REt*(l.-ESQ)/((l.-ESq»*SINLT*3INLT)****l.5)  ♦ ALT 
PPsRE/S0RT((l.-ESQ*3INLTttSXNLTn  ♦ ALT 
HOING=(  IYAW-XWANOR  VRDTOOG 
MAKOERalWANOR/ftOTOOG 
SALF^SINl WANDER) 

CALFtsCOS(  WANDER ) 

C 

c 

DO  $12  xn»3 
, WEL(I)aiVEUI) 

512  CONTINUE 
C 

PITCHalPITCH/ROTODG 

VAWsIVAW/ROTODG 

ROLL*IROLL/RDTOD3 

C 

PDOTeO.O 

YDOTeO.O 

RDOToO.O 

C 

W(l)s-VEU2)/RH 

W(£)oVEUl)/«P 


00002410 

00002420 

00002430 

00002440 

00002450 

00002460 

00002470 

00002460 

00002490 

00002500 

00002510 

00002520 

00002530 

00002540 

00002550 

00002560 

00002570 

00002560 

00002590 

00002600 

00002610 

00002620 

00002630 

00002640 

00002650 

00002660 

00002670 

00002660 

00002690 

00002700 

00002710 

00002720 

00002730 

00002740 

00002750 

00002760 

00002770 

00002760 

00002790 

00002600 

00002610 

00002620 

00002630 

00002640 

00002650 

00002660 

00002670 

00002860 

00002690 

00002900 

00002910 

0C002920 

00002930 

60002940 

00002950 

00002960 

00002970 

00002980 

00002990 

00003000 
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W13»=WI2)«SINLT/C0SLT 

COMPUTE  INITIAL  FORCE  AB  IN  ENU  FRAME 

GR=-l 32 . 0877057* . 16939081*SINLT*SINLT*7. 52810E-6* 

X S1NLT**6 )»(!•-( 9.6227E-8  -6.6089E-10*SXNLT»SINLT>* 

X ALT*6.8512E-15<tAL7HALT) 

GN*1.63E-8*ALT*-SINLT*COSLT 
ABPU  1=0.0 
ABPI  2 1=GN 
A8P( 3 )=GR 

GO  COMPUTE  INITIAL  INERTIAL  TO  BOOT  MATRIX  (QBI) 

GO  TO  800 
600  CONTINUE 

CALL  MXVIQBP.ABP.AB) 

COMPUTE  INITIAL  BOOT  RATE  WST  INERTIAL  ANO  TRANSFORM  TO  BODY  FRAME 

WBP(l)=-COSLN"VEL<2!/RH 
WBPI  2 )=VEL(  1 )/(COSLT=RP 1 ♦ ME 
UBF(3)=SINLN*VEL121/RM 
CALL  MXVIQBI.WBP.WO) 

AOO  INITIALIZATION  TO  PFILE 

FDATAt I 1=WE 
POATAI 2 )=RE 
POATA(  3 )=G 
POATAI A )=PRNTOT 
POATAI 5 )=LAT 
POATAI 6 l=LON 
POATAt 7 1=WANDER 
POATAI 0>=ALT 
POATAt  9 l=ROLL 
POATAI 10  1=PITCH 
POATAI 11 )=YAW 
POATAt 12 )=ROOT 
POATAI 13 J=POOT 
POATAI 16 )=YOOT 
POATAt 15  >°VEL(  1) 

POATAI  16  laVELI  21 
PDATAI171=VEL(3) 

POATAI  18 ISABU) 

POATAt 19 )=A8I 21 
POATAI  20 1BABI31 

NRXTEI PFILE, 1000)  1 1 iPOATAI I ) .1=1.20 1 


INITIALIZATION  OUTPUT  ANO  PRINT  CONTROL 

URITEIOFILE.IOIOIOT.PHNTSU.OUTSN.OFILE.MODPOT.PRNTOT. 
X X UT . X LON,  I ALT . IVE  L . WOLL . 1PITCH , I Y AM,  1 MANOR , AB , MB 


INITSHal 

YTRJ=Y»OT 

RETURN 


00003010 

00003020 

00003030 

00003060 

00003050 

00003060 

00003070 

00033080 

00003090 

00003100 

00003110 

00003120 

00003130 

00003160 

00003150 

00003160 

00003170 

00003160 

00003190 

00003200 

00003210 

00003220 

00003230 

00003260 

00003250 

00003260 

00003270 

00003200 

00003290 

00003300 

00003310 

00003320 

00003330 

00003360 

00003350 

00003360 

00005370 

00003380 

00003390 

00003600 

00003610 

00003620 

00003630 

00003660 

00003650 

00003660 

00003470 

00003680 

00003690 

00003500 

00003510 

00003520 

00003530 

00003560 

00003550 

00003560 

00003570 

00003580 

00003590 

00003600 
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c 

c 

600  CONTINUE 

COMPUTE  EARTH  TO  PLATFORM  TRANSFORMATION  MATRIX  COPE) 

QPEdl-COSLN 
QPE{2 )=0. 0 
QPEI3)=»SINLN 
QPE(4)=-$INLT*SINLN 
QPE15)=C0SLT 
<JPE(6)=-SINLT«COSLN 
QPE(7)=CCSLT«SIMLN 
QPE(6)=SINLT 
GPc(9)=C0SLT«CQSLN 
WRITE  (OFILE.I300)  QPE 

COMPUTE  PLATFORM  TO  BODY  TRANSFORMATION  MATRIX  (QBP) 

ONLY  COMPUTED  DURING  INITIALIZATION  OR  WHEN  YOOT  IS  NOT  ZERO 

IF  (INITSW  .EG.  1)  GO  TO  605 
QEK  2 )"0.0 
QEI(4)  = 0.0 
QEK5I-1.0 
QEII6  )=0.O 
QEKSleO.O 
QCP(3)=0.0 
(!CP(6)=0,0 
QCP(7)=0.0 
QCP(6)=0.0 
QCP(9)=1.0 

SP=SIM<  PITCH  I 
CP=C03< PITCH) 

SR=SXN(POLU 
CR»C03(R0LL) 

SPS^SP^SH 
SPCR=SP*CR 
GO  TO  610 

605  CONTINUE 

IF  (YOOT  .EQ.  0.0)  GO  TO  615 

610  CONTINUE 
SY=SIN( YAW) 

CY=COS(VAW) 

QBC(l)=SYttCP 
GBCt  2 (aCYttCP 
<S0C(3)=SP 

oactois-syspsR-cywcfl 
<50C(5)=-CY«SPSR*3Y*tCR 
<jac(6»=CP*SR 
Q3C(  7 )=“SY**3PCR'»CY#SR 
Qaooia-CYWSPCR-SY^SR 
MC<  9)#CP>CR 
WRITE  (OFILE.1300)  QM 


COMPOTE  INERTIAL  TO  EARTH  TRANSFORMATION  MATRIX  CQEI) 


00003610 

00003620 

00003630 

00003640 

00003650 

00003660 

00003670 

00003660 

0000369U 

00003700 

00003710 

00003720 

00003730 

00003740 

00003750 

00003760 

00003770 

00003760 

00003790 

00003600 

00003610 

00003620 

00003630 

00003640 

00003850 

00003660 

00003070 

00003660 

00003890 

00003900 

00003910 

00003920 

00003930 

00003940 

00003950 

00003960 

00003970 

00003960 

00003990 

00004000 

00004010 

0000402C 

00004030 

00004090 

00U04050 

00004060 

00004070 

00004000 

00004090 

00004100 

00004110 

00004120 

00004130 

00004140 

00004150 

00004160 

00004170 

00004100 

00004190 

00004200 
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c 

00004210 

815  CONTINUE 

00004220 

SWET=SIN(WE»T) 

00004230 

CWETsCOS(WE**T) 

00004240 

QEI(1)=CWET 

00004250 

QEII3)=-SWET 

00004260 

QEI(7)=SWET 

00004270 

qe:i9)=cwet 

00004260 

c 

WRITE  ( OFILE ,1300 ) QEX 

00004290 

c 

00004300 

c 

COMPUTE  NORTH  POINTING  TO  UANOER  PLATFORM  MATRIX  (Qf?) 

00004310 

c 

00004320 

QCP(1)=CALF 

00004330 

QCPI2 JsSALF 

00004240 

CCP(4)=-3ALF 

00004350 

CCPISlaCALF 

00004360 

c 

WRITE  (OFILE. 1300)  QCP 

00004370 

c 

00004360 

c 

COMPUTE  INERTIAL  TO  BOGY  TRANSFORMATION  MATRIX  (Q3I) 

00004390 

c 

000044C0 

CALL  MXM( Q3C .QCP.Q8P) 

00004410 

c 

WRITE  (OFILE. 1300)  OBP 

00004420 

CALL  MXMIQ3P.QPE.QTEMP) 

00004430 

c 

WRITE  (OFILE. 1300)  GTE  HP 

00004440 

CALL  MXM( QTEMP.QEI »Q3X 1 

00004450 

c 

WRITE  (OFILE. 1300)  CSX 

00004460 

c 

00004470 

IF  (XNITSM.EQ.l)  GO  TO  200 

00004400 

00  TO  600 

00004490 

c 

03004500 

c 

00004510 

1000  FQUHAY  (IS.F20.10) 

00004550 

10X0  FORMAT  1 30H  TRAJECTORY  INITIALIZATION  , 

00004530 

X/SX.OH  OT  .3X.016.0i3X.4H  SEC. 

00004540 

X/3X.3M  PPNTSH  .3X.G16.0. 

00004550 

X/1X.6H  OUTSW  .3X.G16.0. 

00004560 

X/3X,ei(  OFILE  .3X.X1S, 

00004570 

X/3X.0H  MOOPDT  .3X.GI6.0. 

00004500 

X/3X.8H  PSNTOT  .3X.G16.6.X/. 

00004590 

X/3X.1SH  LAT(OEG)  .SX.G16.0, 

00006600 

X73X.15H  LON(OEG)  .3X.G16.6. 

00004610 

X/SX.15H  ALT(FT)  .SX.GIO.O. 

00004620 

X/SX.1SH  VEH  FT/SEC)  .3X.SG16.6,//, 

00004630 

X.3X.1SH  COLLI 0CC3)  .016.0. 

00906640 

X/3X.1SH  PnCH(OEGS)  .016. 0, 

00004650 

X/3X.1SH  YAHtOEGS)  .016.0, 

00004660 

X/3X.15M  HAMPER! OtGS)  ,016.0. 

00004670 

X/3;(.1SH  AQ(FT/3EC2)  .3016.0* 

00004680 

X/3X.1SH  W31PA0C/SEC)  .3016.0.//) 

00004690 

UOO  FORMAT! 2X. 016.01 

09004700 

1200  FCmU.TUX.2SM  «t»  TRJ  *•  A8trT/$EC2) 

.3016.0./ 

00009710 

X 6X.CSH  WSt RAD/SEC) 

.3016.0,/ 

00004720 

X 6X.3SH  LAT(OEGS) 

•G16.6,/ 

00004730 

X 6X.25II  LOUtOEGSI 

.016.0./ 

00004740 

X 6X.25H  ALFIOEGS) 

,Gi6.e./ 

09004750 

X 6X.2SH  ALY(FY) 

•G16.S,/ 

00009760 

* 

X eX.SSH  VEUFT/SEC) 

,3016.0./ 

00004770 

X 6X.2SH  OVIFT/SEC) 

.3G16.0./) 

00009700 

1100  FORMAT! 1CX.6H  <531  ,3G16.0,/24X.3C16.6./24X.3G;6.8«//) 

00004790 

1310  F0RMAT(10X.6H  GOX*  ,3S16.8./24X.3G16.fl./24X,3G16.0,//) 

0000480C 

ENO 

00004010 
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o no  o o « 


00000010 

05/05/70  DATE  OF  CURRENT  MODULE  00000020 

00000030 

THE  ENVIRONMENT  MODULE  IMPOSES  VIBRATIONAL  DISPLACEMENTS  UPON  THE  00000040 
POSITION.  ATTITUDE.  AND  VELOCITY  COMPONENTS  OF  THE  TRAJECTORY  00000050 

00000060 

SUBROUTINE  ENV  (T.1EN0F.AB.U3.  00000070 


X 

A03.WBO.H8EDOT ) 

00000060 

REAL 

A 

00000090 

REAL 

ABI31 

00000100 

REAL 

ABBI3) 

03000110 

REAL 

AP 

00000120 

REAL 

B 

00000130 

REAL 

C 

00000140 

REAL 

C0SFN30) 

00000150 

REAL 

CQEF2(iO) 

00000160 

REAL 

COEF3130) 

00000170 

REAL 

C0EF4130) 

00000100 

REAL 

COEF5(3Q) 

00000190 

REAL 

C0EF6I 30 ) 

00000200 

REAL 

COEF  7(301 

00000210 

REAL 

OATA(IOS) 

00000220 

REAL 

DELAOt  J> 

00000230 

REAL 

OELVQt  3) 

00000241) 

REAL 

OELWOPI 3) 

00000250 

REAL 

DELVBPI31 

00000260 

PEAL 

OELUOISI 

00000270 

REAL 

OT 

00000200 

REAL 

i 

00000290 

REAL 

El 

00000300 

REAL 

ETA 

00000310 

REAL 

F 

00000320 

HEAL  FA  00000330 

HEAL  Ft  00000340 


REAL 

0 

00000350 

REAL 

GAS 

OOOOO’iOO 

HEAL 

GAUSS 

00000370 

REAL 

tUiVt  A 1 

00000300 

REAL 

N 

00000393 

REAL 

HOOPOE 

00000400 

REAL 

P 

00000410 

REAL 

Pll 

ooooewo 

REAL 

PU 

00000430 

REAL 

P21 

00000490 

REAL 

P22 

00000450 

REAL 

RaRHAPI  30 1 

00000460 

REAL 

Pain  ttoitso) 

00000970 

REAL 

PARHH3I  30 1 

00000480 

REAL 

peuFutJ 

00000450 

REAL 

FOATA(fcO) 

00000500 

REAL 

PI 

00000510 

REAL 

FHNTDT 

oooaasso 

RIAL 

S&0591 

00090530 

REAL 

5C0EHS«9) 

00000540 

REAL 

RgOPSVIRI 

00000550 

REAL 

QTEHPI91 

00000500 

REAL 

4TQ(9I 

00000570 

REAL 

R 

COOOOS0O 

REAL  RAKDI6I  08300540 

REAL  RE  COOOCAOO 


REAL  S<6) 

REAL  SA 
REAL  SPARE1 
REAL  T 
REAL  TPRHOO 
REAL  TPRNT 
REAL  TEMP 
REAL  TENV 
REAL  U 
REAL  V 
REAL  WB(  3) 

REAL  M3B(3) 

REAL  KBBOLD( 3) 

REAL  KBBDOTO) 

REAL  ME 
REAL  HH 
REAL  WN 
REAL  WO 
REAL  MR 
REAL  WTEMPl 9) 

REAL  Xl(5) 

REAL  X1PRV(30) 

REAL  X2(5) 

REAL  X2PHV(  30 ) 

REAL  Y 
REAL  Z 
REAL  ZETA 

INTEGER  IENDF 
INTEGER  INITSM 
INTEGER  IX 
INTEGER  OUTSH 
INTEGER  IFILE 
INTEGER  PFILE 
INTEGER  OFILE 

EQUIVALENCE  (POATAdb  t!“) 
EQUIVALENCE  (PDATAIEb  RE) 
EQUIVALENCE  <PDATA<3),  G) 
EQUIVALENCE  (POATAIAW  PRNTOT) 
EQUIVALENCE  (POATA(S).  PBUF(U) 

EQUIVALENCE  (DaTA(X),  DT) 
EQUIVALENCE  (0ATA(2)>  PRNTSW) 
EQUIVALENCE  (DATA(3)i  OUTSW) 
EQUIVALENCE  (DATA! A),  XFILE ) 
EQUIVALENCE  (DATA(S)*  S(D! 
EQUIVALENCE  (DATA(6)>  3(2)) 
EQUIVALENCE  (DATA17).  3(3)) 
EQUIVALENCE  (OATA(S),  St  A)) 
EQUIVALENCE  (DATA(9)»  3(5)  > 
EQUIVALENCE  (DATA(lQ)i  3(6)) 
EQUIVALENCE  (DATA(ll),  PARHAP(l)) 
EQUIVALENCE  <DATAU2)»  PARHAPI2) ) 
EQUIVALENCE  (DATA(lS)t  PARHAP(3')> 
EQUIVALENCE  tOATAtlAb  PARMAPtA)) 
EQUIVALENCE  (DATA! 15).  PARHAP(5)) 
EQUIVALENCE  (0ATAU6),  PARMAP(6!) 
EQUIVALENCE  (DATA! 17 )i  PARMAPI7) ) 
EQUIVALENCE  (OATA(IB).  PARMAP<6>> 


00000610 
000006^0 
00000630 
000006AO 
00030650 
00000660 
00000670 
00000660 
00000690 
00000700 
00000710 
00000720 
00000730 
000007AO 
00000750 
00000760 
00000770 
00000730 
00000790 
00000800 
00000B10 
00000320 
00000830 
OOOOOSAO 
00000850 
00000860 
00000370 
00000830 
00000890 
00000900 
00000910 
00000920 
00000930 
OOOQB9AO 
00000950 
00000960 
0000  0970 
00000960 
00000990 
0000X000 
0000X0X0 
00001020 
00(501030 
OOOOXOAO 
0000X050 
00001060 
0000X070 
00001080 
00001090 
0000X100 
00C01UQ 
0000X120 
0000X130 
OOOOXXAO 
00001150 
00001160 
00001170 
00001160 
00001190 
00001200 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

equivalence 

equivalence 

equivalence 

equivalence 


<&ATA(19>: 
<OATA<20), 
(DATA! 21) 
(DATA(22), 
(0ATA123), 
(DATAC24), 
(QATA(25), 
(DATA!  26); 
(QATAt  27) , 
(DAT<*i28), 
l DATA!  29), 
(DATA!  30), 
(DATA)  31)  i 
(DATA(32), 
(DATA! 33) , 
(DATA(36), 
(DATA! 35)  i 

< DATA!  36), 
t DATA( 37 ) , 
<0ATA(38), 
(QATA(  39 ) , 
(DATA160), 
(DATAI61), 
(DATA(62), 
(0ATA(63), 
<0ATA<66>, 
(DATA)  AS), 
(0ATAIA6), 
(DATAtAI) i 
(DATA!  A8)i 
(DATA169) , 
(DATA  (50)  i 
(QATAt 51) , 
( l)ATA(  52 ) i 
(0ATA(53) i 
(DATA!  5A), 
* DATA!  55), 
(DATA) 56) i 

< OATAt  57)  i 
(0ATA(5Gi, 
(0ATA(59), 
(DATA! 60) , 
( DATA (61), 
(DATA* 62 h 
(DATA!  63), 
<0ATA(6A), 
(DATA!  65)  i 
I DATA!  66), 
(DATA!  67), 
(DATA!  66), 
(0ATA(69)i 
(OATAtTO), 
(OATA(Tl)i 
tDATA(72)i 
( OATH  T3 ) i 
tOATA(76), 
(0ATA(T5), 
(OATA(76), 
<OATA(77), 
(0ATAL76), 


PATMAP(9) ) 
PASIMAPtlO) ) 
PARHAPt 11 ) ) 
PARflA,'*!  12 ) ) 
PARHAPt 13)) 
PARHAPt  16)) 
PARHAPt  15) ) 
PAR MAPI  16 ) ) 
PARHAPt 17 ) ) 
PARHAPt  2.8  J ) 
PARHAPt 19 ) ) 
PARMAPf 20 ) ) 
PARHAPt  21 ) ) 
PAR HAP ( 22 ) ) 
PARHAPt  23) ) 
PARHAPt 26 ) ) 
PARIIAPI  25)) 
PARHAPt  26)) 
PARHAPt 27)) 
PARHAPt 28)) 
PARHAPt 29)) 
PARHAPt 30)) 
PARHWHU)) 
PARMUHI2) ) 
PARHWNt 3)) 
PARHWHt  6 ) ) 
PARHWHt 3) ) 

P ARM  Wilt  6 ) ) 
PARHWNt  7) ) 
PARHWHI8I) 
PARHUHI 9) ) 
PARHWNt 10)) 
PARHWNt  ID) 
PARHWNt 12)) 
PARHWHt 13)) 
PARHWNt 16 ) ) 
PARHWNt 15)) 
PARHWNt 16)) 
PARHWHt 17)) 
PARHWNt 16)) 
PARHWNt 19)) 
PARHWNt 20)) 
PARHWNt  21 ) > 
PARHWHt 22 >> 
PARHWNt 23)) 
PARHWNt 26)) 
PARHWNt  JS) ) 
PARHWNt  26)) 
PARHWNt  27)) 
PARHWNt 26)) 
PARHWNt  i<> ) ) 
PARHWHt 30 )> 
pARHWOt 1 ) ) 
PARIWQIE)) 
PARHWOt 3)) 
PARHWOt  6 • ) 
PARHWOt  5)) 
PARHWOt  6 ) ) 
PAPHWOt  7) ) 
PA)stQ»Ql  6 ) ) 


00001210 
00001220 
00001230 
00001260 
00001250 
00001260 
00001270 
00001260 
00001290 
00001300 
00001310 
00001320 
00001330 
00001360 
00001350 
00001360 
00001370 
00001380 
00001390 
00001600 
00001610 
00001620 
00001630 
00001660 
00001650 
00001660 
00001670 
00001660 
0" 001690 
00001500 
00001510 
00001520 
00001530 
00001560 
00001550 
00001560 
00001570 
00001560 
00001590 
00001600 
00001610 
00001620 
00001630 
00001660 
00001650 
00001660 
00001670 
00001660 
00061690 
00001700 
00001710 
00001720 
00001730 
00001760 
00001750 
00001760 
00001770 
00001760 
00001770 
00001600 


no  nnnnn  nnnnn 


C 


c 


c 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 


(OATAt 79) 
(DATA (80) 
(DATA! 81) 
(OATAt 82 ) 
(DATA(SJ) 
(OATAt 84) 
(OATAt 85) 
(OATAt 86) 
(0ATA(67) 
(OATAt 88) 
(OATAt 89) 
(OATAt  90 ) 
(OATAt 91) 
(OATAt 92) 
(OATAt 93) 
(OATAt 94) 
(DATA( 95) 
(DATM96) 
(OATAt 97) 
(OATAt 98) 
(DATA! 99) 
(OATAt 100) 


PARMWOt 9 ) ) 
PARHWOI 10 ) ) 
PARHWOI 11 ) ) 
PARHWOI 12 ) ) 
PARHWOI 13)) 
PARMWOt 14) ) 
PARHWOI 15)) 
PARHWOI  16 ) ) 
PARHWOI 17)) 
PARMWOt 18 ) ) 
PARMWOt 19)1 
PARMWOt 20 )) 
PARHWOI 21)) 
PARHWOI 22 ) ) 
PARHWOI 23)) 
PARMWOt 24)) 
PARHWOI 25)) 
PARMWOt  26 ) ) 
PARHWOI  27)) 
PARHWOI 28 ) ) 
PARMWOt 29)) 

» PARHWOI 30)) 


EQUIVALENCE  (OATAt 101),  MODPDT) 
EQUIVALENCE  (OATAt 102).  VIBSW) 
EQUIVALENCE  (OATAt 103) .GUSTLA) 
EQUIVALENCE  (DATA(104),GUSTLO) 
EQUIVALENCE  ( OATAt 105 ) , GUSTNR ) 


DATA  DELWBP/3*0./ 

DATA  DELVBP/3*Q./ 

DATA  PX/3. 1415926535397/ 

DATA  QSBPRV/1. 0,3*0. 0,1. 0,3*0. 0,1.0/ 
DATA  RAN0/6*0.0/ 

DATA  TPHM03/0.0/ 

DATA  TPRNT/0.0/ 

DATA  INITSW/0/ 

OATA  INI  /0/ 

OATA  IN2  /0/ 

OATA  If-  HE/30/ 

OATA  PPILE/7/ 

IF  tlENQF.EQ.l)  RETURN 
IF  (INITSW.IQ.O)  GO  TO  500 
IF  (T.LT.TENV-.OOOS)  RETURN 
100  CONTINUE 


IF  VIBRATION  SWITCH  13  ZERO  BYPASS  VIBRATION  COMPUTATION 
AND  ADD  ZERO  RATE  ANO  ACCEUTXON 

IF  (VIBSW. LT.l)  GO  TO  949 

THE  FOLLOWING  GENERATES  SEQUENCES  OF  RANDOM  NUMBERS 
Xt VIBRATIONS ) CORRESPONDING  TO  DESIGNED  PSD'S 


DO  940  J*1.6.I 


00C01810 

00001820 

00001830 

00001840 

00001850 

00001860 

00001870 

00001830 

00001890 

00001900 

oooomo 

00001920 

00001930 

00001940 

00001950 

00001960 

00001970 

00001980 

00001990 

00002000 

00002010 

00002020 

00002030 

00002040 

00002050 

00002060 

00002C70 

00002000 

00002090 

00002100 

00002110 

00002120 

00002130 

00002140 

00002150 

00002160 

00002170 

00002180 

00002190 

00002200 

00002210 

00002220 

00002230 

00002240 

00002250 

00002260 

00002270 

00002280 

00002290 

00002300 

00002310 

00002320 

00002330 

00002340 

00002350 

00002360 

00002370 

00002380 

00002390 

00002400 


1-22 


i 


Y=0.0 

00002610 

N=S(J) 

00002620 

DO  930  XS1,N»1 

00002630 

C 

00002660 

£TA=GAUSS  (0.0, 1.0) 

00002650 

ZETA=GAUSS  (0.0, 1.0) 

00002660 

C 

00002670 

X=5*(J-1)  *1 

00002*80 

C 

00002690 

C 

00002500 

X1(I)=C0EF6(K)*X1PRV(K)  + COEF6(K)*X2PRV(K)  ♦ 

00002510 

XCOEF2(K)*ETA  ♦ C0EF1(K)*ZETA 

00002520 

X2(I)=C0EF7(K)#X1FRV(K)  ♦ COEF5CK)*SX2PRV(K)  ♦ 

00002530 

XC0EF3(K)«ETA 

00002560 

1F(1N2.NE.0)  GOTO  925 

00002550 

Y=Y+X1(I)/DT 

00002560 

GOTO  926 

00002570 

925  CONTINUE 

00002580 

Y=Y*(X1(  I J-XIPRV(K)  )/OT 

00002590 

926  CONTINUE 

00002600 

X1FRV(K)=X1(I) 

00002610 

X2PRV(K)=X2(I) 

00002620 

C 

00002630 

930  CONTINUE 

00002660 

RANDtU  )=Y 

00002650 

960  CONTINUE 

00002660 

C 

00002670 

IN2=1 

00002680 

C 

00002690 

C VIBRATION  GENERATOR  COMPLETE 

00002700 

C 

00002710 

C COMPUTE  ENVIRONMENT  OUTPUT 

00002720 

C 

00002730 

00  950  1=1,3 

00002760 

K=I*3 

00002750 

WSBOUHIlsWBBlI) 

00002760 

OELVB(I)=RAND(J) 

00002770 

OELABI  I )=(  0EIV0(  I )-DELVBP(  I ) >/0T 

00002700 

QELVBPt X )=OELVB( X) 

00002790 

OELWBU)BRANO(K) 

00002800 

950  CONTINUE 

00002810 

XN1=1 

00002820 

WBB(  1 )=WB(  1 HDELUBI 3) 

OQ002830 

UD3( 2 )=UB( 2 )*DELWB(  1 ) 

00002060 

WBBt 3)=WB( 3)»DEIMB(  2 ) 

00002050 

C COMPUTE  THE  CHANGE  IN  BODY  ATTITUDE  OUE  TO  VIBRATION 

00002860 

QBBCH6U>"1. 

00002070 

QBOCHGI  2 )=(  DELMBI  2 ) ♦DELWBPt  2 ) )/2.*DT 

00002880 

QBBCHGI  3)=-(DELWB(  1 WDELWBPt  1 ) )/2.»DT 

00002890 

QBBCHGI 6) =-IDELW3(  2 WDELW3PI  2 ) )/2.#DT 

00002900 

QBBCHGI 5 )al. 

00002910 

QBBCHG!  6 )=<  0ELW3I  3 )*DELWBP<  3 ) )/S  >0T 

00002920 

QDBCHGI  7)=(0ELUB(  1 )*DELWBP(  1 ) >/C ,*DT 

00002930 

QBBCHG(6)=-(DELWB(3)*DEIWBP(3))/2.«DT 

00002960 

QBBCHGI 9)31. 

00002950 

, CALL  MXM( QBBCHG, QBBPRV.QOB) 

00002960 

. DELWBPt  l)aDELMB(l) 

00002970 

DELUBPf  2 )=DE LWBt 2 ) 

00002900 

DEUs'BPI  3)=DELWB(  3) 

00002990 

C ORTHONORHALIZE  THE  ATTITUDE  MATRIX 

00003000 
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CALI  MTXMi QBB  * QBB » QTQ ) 

00003010 

00  952  19=1,9 

00003020 

QTQt  19 ) =- , 5#QTG(  14 ) 

00003030 

952  CONTINUE 

00003090 

QTQ(1)=1.5+QTC(1> 

00003050 

QTQ(5)=1.5+QTQ(5) 

00003060 

QTQt 9 )=1 .5+QTQt  9) 

00003070 

CALL  MXMt  GBB , QTQ > QTEMP ) 

06003080 

00  953  15=1.9 

00003090 

QBB(I5)=QTEMP(I5) 

00003100 

953  CONTINUE 

00003110 

C ENO  QRTHQNORMALIZATION 

00003120 

C CALCULATE  ROTATED  ACCELERATION 

00003130 

CALL  MXV(QBB,AB,ABB) 

00003190 

DO  959  1=1,3 

00003150 

WESDOTt  I )=(  WBB(  I )-WBBOLD(  I > )/0T 

00003160 

959  CONTINUE 

00003170 

DO  955  1=1,9 

00003180 

QBBPRVt I )=QBB(  I J 

00003190 

955  CONTINUE 

00003200 

ABBt H =ABB(1 ) *D E LABt 3 ) #G 

00003210 

ABB(  2 )=ABB(  2 HDELABt  2 )*G 

00003220 

ABB!3>=ABB(3)*0ELABtl)»G 

00003230 

GOTO  “56 

00003290 

999  CONTINUE 

00003250 

DO  957  1=1,3 

00003260 

WBDOLD(I)=WBB(I) 

00003270 

WBB(I)=WB(I) 

00003280 

ADD!  I )=AB( I ) 

00003290 

WEBOOT ( I ) = ( WBBt I »-WBBOLD( Z ) l/OT 

00003300 

957  CONTINUE 

00003310 

956  CONTINUE 

00003320 

C 

00003330 

C OUTPUT  AND  PRINT  CONTROL 

00003390 

C 

00003350 

IF  (BRNTOT.GT.O)  00  TO  960 

00003360 

IF  (HODPOT.EQ.O)  00  TO  999 

00003370 

C 

00003380 

IF  (T.LT.TPRMOD*.O0O5)  60  TO  999 

00003390 

TPRMOD=TPRMOD*MODPOT 

00003900 

GO  TO  970 

00003910 

C 

00003920 

960  CONTINUE 

00003930 

IF  t T . LT . TPRNT -.0005)  GO  TO  999 

00003990 

TPRNT=TPRNT+PRNTOT 

0001  950 

c 

OOOOj^O 

970  CONTINUE 

00003970 

IF  (PRNTSW.LT.l)  GO  TO  999 

00003980 

C 

00003990 

WRITE (OFILE, 1200)  AB , WB , ABB , WBB , W8BD0T 

00003500 

C 

00003510 

C 

00003520 

c 

00003530 

999  CONTINUE 

00003590 

TENV=T*DT 

00003550 

RETURN 

00003560 

.c 

00003570 

C ENVIRONMENT  INITIALIZATION  BEGINS  HERE 

000035S0 

C 

00003590 

500  CONTINUE 

00003600 

I 
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REWIND  IFILE 
REWIND  PFILE 

501  READ  ( IFILE ,1000 ) IX.DATA(IX) 

IF  (EOF(IFILE))  502,501 

502  CONTINUE 

505  READ  (PFILE, 1000)  IX.POATAUX) 

IF  (EOF( PFILE))  510,503 
510  CONTINUE 

REWIND  IFILE 
REWIND  PFILE 
C 

OFILE=XFILE 

C 

C 

C THE  FOLLOWING  GENERATES  INITIAL  VALUES  FOR  RANOON  NUMBERS 
C HAVING  SPECIFIED  TURBULENCE  PSD'S 
C 

C GUST  IS  IN  UNITS  OF  FT/SEC  AND  REPRESENTS  VARIANCE  OF  WIND  VELOCITY 
GUST( 1 JsGUSTNR 
GUST(2)=GUSTLA 
GUST(3)=GUSTLO 
G'JST(4)=GUSTNR 
GUST(5)=GUSTLA 
GUST(6)=GUSTLA 
C 

L=1 

C 

DO  600  J=l,6,l 
N=S( J ) 

IF  (N.ED.O ) GO  TO  600 
DO  605  IS1,N,1 
K-5»U-1HI 
C 

ETA=GAUSS  (0.0, 1.0) 

ZETA=GAU3S  (0. 0,1.0) 

C 

ApaPARMAP(K) 

WHaPARMWHtK) 

WO=PARMWO(K) 

C 

APsGUST(J)  *2*PX»AP 

AsKH/WO 

$A=A*»2 

TEMPbSQRT(2-(1-SAX2)«»2) 

WNaWOttSQRT(TEMP) 

EaSQRTl  ( l-( WO/WN )*#£  )X2 ) 

F3AP#(l-<l-2#e»»2>#»2) 

XlPRV(K)B0. 

X2FRV(K)*0. 

WR=WN**8QRT<  1-E#“2) 

Z=E*WN 

ElBEXP(-2*Z*OT) 

CeASIN(E) 

C 

Us(l-El><'(4»Z»WR#»2)+ 

XI E 1*><  Z»CQS<  2<tWR#DT  >*WR#SXN(  2*»WR«DT ) ) >/(  4*<  WN<H»2 )«( WR*»2 ) ) 
X-E/<4*WN»WR»M»2) 

C 

RMl-El )/( ( 4*E*WR##2 1/WN1- 

X( El»l Z«COSt 2WWH#DT+2»C )-WRmSIN( 2#WR«OT*2«C ) ) )/<4«URtt»2 ) 


00003610 

00003620 

00003630 

00003640 

00003650 

00003660 

00003670 

00003680 

00003690 

00003700 

00003710 

00003720 

00003730 

00003740 

00003750 

00003760 

00003770 

00003780 

00003790 

00003000 

00003810 

00003820 

00003830 

00003840 

00003850 

00003360 

00003870 

00003880 

00003690 

00003900 

00003910 

00003920 

00003930 

00003940 

00003950 

00003960 

00003970 

00003980 

00003990 

00004000 

00004010 

00004020 

00004030 

00004040 

00004050 

00004060 

00004070 

00004080 

00004090 

00004100 

00004110 

00004120 

00004130 

00004140 

00004150 

00004160 

00004170 

00004180 

00004190 

00004200 


t 
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X+t  Z*C0S(  2*C  )-WR»SIN<  2#C ) )/! 4*WR**2 ) 

C 

V=<  El-1  >#SIN<  C )/( 4*E#WR»*2  )- 

X!  E1*(Z*$IN!  2*WR*OT+C)+WR«COS(  2#WR*DT+C ) ) )/(4*WN«WR**»2 ) 

Xit WR»COS( C )+Z#5INl C ) )/<  4*WN*WR»*2 ) 

C 

B=S0RT(  ABS!  U-(  V**2  )/R ) )*SQRT<  F )#WN#*2 
H=( V/SQRT( R I )«SQST( F )*WN#WN 
PstSQRTtR ) )#SQRT(F )*WN#»2 
C 

P12a($IN(WR#0T)#EXP!-Z*CT))/WR 
P223( COS( WR#0T l-( Z/WR )*SINl WR*DT ) )*EXP(  -Z*DT ) 
P11=2*Z*P12»P22 
P21s(  -WN***2  )*P12 
C 

C0EF1!K)=B 
C0EF2!K)=N 
C0EF3(K)=P 
C0EF4(K)*P12 
C0EF5(K)!P22 
C0EF6! K )rPll 
COEF7!K)=P21 
C 

605  CONTINUE 
600  CONTINUE 

PSD’S  COEF  PARAMETERS  COMPUTED  FOR  A SET  OF  AMPLITUDE, 
HALF-WIDTH,  RESONANT  FREQUENCY  AND  GUST 

DO  610  in. 3 
WDBtI)*WB<I) 

AEB! I )=AB! 1 1 
WBBDOT! 1 1=0.0 
610  CONTINUE 

INITIALIZATION  OUTPUT  AND  PRINT  CONTROL 
WRITE!  OFILE, 1010 )DT ,PRNTSW,OUTSW»OFILE,PRNTOT ,MQQPOT»VIBSW 

700  CONTINUE 
N-$t  1 ) 

WRITE! OFILE ,1020)  N 
J=1 

00  TO  750 

701  CONTINUE 
N=S(2) 

WRITE! OFILE, 1021)  N 
J=2 

GO  TO  750 

702  CONTINUE 
N=S( 3) 

WRITE! OFILE i 1022)  N 

J®3 

00  TO  750 

703  CONTINUE 
NaSI 4) 

WRITE(OFILE,1023>  N 

WTO  750 

704  CONTINUE 


00004210 

30004220 

00004230 

00004240 

00004250 

00004260 

00004270 

00004260 

00004290 

00004300 

00004310 

00004320 

00004330 

00004340 

00004350 

00004360 

00004370 

00004360 

00004390 

00004400 

00004410 

00004420 

00004430 

00004440 

00004450 

00004460 

00004470 

00004460 

00004490 

00004500 

00004510 

00004520 

00004530 

00004540 

00004550 

00004560 

00004570 

00004560 

00004590 

00004600 

00004610 

00004620 

00004630 

00004640 

00004650 

00004660 

00004670 

00004660 

00004690 

00004700 

00004710 

00004720 

00004730 

00004740 

00004750 

00004760 

00004770 

00004760 

00004790 

00004600 


I 
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N*S(5> 

00006810 

WRITE! OFILE >1026)  N 

00006820 

Js5 

00006830 

GO  TO  750 

00006860 

705  CONTINUE 

00006850 

NsS(6) 

00006860 

WRITE (OFILE, 1025)  N 

00006870 

J=6 

00006880 

GO  TO  750 

00006890 

706  CONTINUE 

00006900 

WRITE!  OFILE >1026) 

00006910 

C 

00006920 

INITSWsl 

00006930 

TENV=T+0T 

00006960 

RETURN 

00006950 

C 

00006960 

C 

00006970 

750  CONTINUE 

00006980 

IF  (N.EQ.O)  GO  TO  752 

00006990 

DO  751  1=1, N,1 

00005000 

K=5k(J-1)+I 

00005010 

WR IT  E ( OF I LE « 1 0 30 ) PARMAP ! X ) > P ARHWH ( K ) » P ARMWO  ( K ) 

00005020 

751  CONTINUE 

00005030 

752  GO  TO  ( 701,702,703,706,705,706 ),J 

00005060 

C 

00005050 

C 

00005060 

1000  FORMAT  (I5>F20.10) 

00005070 

1010  FORMAT! //,30H  ENVIRONMENT  INITIALIZATION  , 

00005080 

X/3X.6H  DT  ,3X>G16.8>3X,6H  SEC, 

00005090 

X/3X.6H  PRNTSM  ,3X,G16.8, 

00005100 

X/3X.8H  OUTSW  .3X,G16.8, 

00005110 

X/3X.8H  OFILE  ,3X,I15, 

00005120 

X/'IX.eH  PRNTOT  ,3X,G16.8, 

00005130 

X/3X.8H  MODPDT  ,3X,G16.«, 

00005160 

X/3X,6H  VID3W  ,3X,G16.8>//) 

00005150 

C 

00005160 

1020  FORMAT! 3X.32H  VIBRATION  GENERATOR 

PARAMETERS  ,//. 

00005170 

X 3X.20H  VERTICAL  LOAO  » 

16X.6H  AP  ,8X,6H  WH 

,6X,6H  WO  , 00005180 

X /'6X,I6,7H  PEAKS  > 

00005190 

1021  FORMAT! /3X.20H  LATERAL  LOAD 

# 

00005200 

X /6X,I6,7H  PEAKS  ) 

00005210 

1022  FORMAT! /3X.20H  LONGITUDINAL  LOAD 

» 

00005220 

X /6X,I6,7H  PEAKS  ) 

00005230 

1023  FORMAT! /3X.20H  PITCH  RATE 

• 

00005260 

X /6X»I6,7H  PEAKS  ) 

00005250 

1026  FORMAT! /3X, 2 OH  YAW  RATE  PAD 

• 

00005260 

X /6X,I6>7H  PEAKS  ) 

00005270 

, 1025  FORMAT(/3XiCOH  ROLL  RAYE  PSD 

f 

00005280 

X /6X,I6,7H  PEAKS  > 

00005290 

1026  FORMAT! // ) 

00005300 

C 

00005310 

1030  F0RMAT(27X,G16.8,5X,G16.8,7X>G16.8> 

00005320 

1100  FORMAT! 1H*,8X> 016. 8) 

00005330 

1200  FORMAT! 6X.25H  *»  ENV  M AB(FT/3EC2> 

,3016.6,/ 

00005360 

X 6K.25H  WS( RAD/SEC) 

,3G16.8,/ 

00005350 

X 6X.25H  ABD! FT/SEC2 ) 

,3G16.0,/ 

00005360 

. X 6X.2SH  WDB! RAD/SEC) 

.3G16.8,/ 

00005370 

X 6X.25H  WOBOOT!R/S2) 

•3G16.8,//) 

00005380 

C 

00005390 

C 

00005600 
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1300  FORHAT(6X»6H  TEMP  , SG16.6,/12X.3G16.0,/12X»3316.e,//)  00005410 

1400  FORMAT<3X>3F16.Si/)  00005420 

1410  FORMAT(3X,7F9.6,/»  00005430 

END  00005440 
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03/10/76  DATE  OF  CURRENT  MODULE 

00000010 

00000020 

THE  ACCELEROMETER  MODULE  SIMULATES  A PENDULOUS  SINGLE  DEGREE  OF 

00000030 

00000040 

FREEDOM 

(SDF)  FLOATED  ACCELEROMETER. 

- 00000050 

SUBROUTINE  ACCE  L( T , IENDF , ABB ,WBB , HOOT » 

00000060 

00000070 

X 

OV) 

00000000 

REAL 

ABAI3) 

00000090 

00000100 

REAL 

ABB(3> 

00000110 

REAL 

ABC(3) 

00000120 

. REAL 

ABXASI 3) 

00000130 

REAL 

AQUANT 

00000140 

REAL 

A20 

00000150 

REAL 

BIA 

00000160 

REAL 

BIAS(3) 

00000170 

REAL 

BIA3A1 3) 

00000180 

REAL 

BXASVI 3) 

00000190 

REAL 

CO 

00000200 

REAL 

CROSS1I 3) 

00000210 

REAL 

CROSS2I 3) 

00000220 

REAL 

DATA! 97) 

00000230 

REAL 

DELI 

00000240 

REAL 

DEN 

00000250 

REAL 

DT 

00000260 

REAL 

0THETK3) 

00000270 

REAL 

DTI 

00000260 

REAL 

0V(  3) 

00000290 

REAL 

EX 

00000300 

REAL 

G 

00000310 

REAL 

GAUSS 

00000320 

REAL 

HT 

00000330 

REAL 

I 

00000340 

REAL 

LAT 

00000350 

REAL 

K 

00000360 

REAL 

KOI  3) 

00000370 

REAL 

KP13) 

00000380 

REAL 

KIK3) 

00000390 

REAL 

KPPI3) 

00000400 

REAL 

KIOI3) 

00000410 

REAL 

KOPI3) 

00000420 

REAL 

KIPI3) 

00000430 

REAL 

MOOPOT 

00000440 

REAL 

MRC 

00000450 

REAL 

Ml 

00000460 

REAL 

M2 

00000470 

REAL 

ORDER 

00000480 

REAL 

PBUFI16) 

00000490 

REAL 

POATAtaO) 

00000500 

REAL 

PRNTDT 

00000510 

REAL 

QBAXI9) 

00000520 

REAL 

GRAYIR) 

00000530 

REAL 

QQAZI91 

00000540 

REAL 

QUANT 

00000550 

REAL 

RE 

00000560 

REAL 

RXI3) 

00000570 

REAL 

RYtS) 

00000580 

REAL 

RZI3) 

00000590 

REAL 

SFM0I3) 

00000600 
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REAL  SFMM3) 

00000610 

REAL  SPP013) 

00000620 

REAL  SFPlt3) 

00000630 

REAL  SM0(3) 

00000690 

REAL  Stilt  3) 

00000650 

REAL  SPOt 3) 

00000660 

REAL  SP1(3) 

00000670 

REAL  SPARE1 

00000680 

REAL  SPARES 

00000690 

REAL  T 

00000700 

REAL  TACC 

00000710 

REAL  TDTHET 

00000720 

REAL  THOOT(3) 

00000730 

REAL  TKETA(3) 

00000790 

REAL  TNN 

00000750 

REAL  TPRMOO 

00000760 

REAL  TFRNT 

00000770 

REAL  TT 

00000780 

REAL  TTHETt  3) 

00000790 

REAL  WBAt  3) 

00000800 

REAL  NSBt  3) 

00000810 

HEAL  MDOTt  3) 

00000820 

REAL  KD0TA(3J 

00000830 

REAL  RE 

00000890 

00000850 

INTEGER  IEN0F 

00000860 

INTEGER  INITSW 

00000670 

INTEGER  IFILE 

00000880 

INTEGER  OFILE 

00000690 

INTEGER  OUTSW 

00000900 

INTEGER  PFILE 

00000910 

00000920 

EQUIVALENCE 

(DATA(l)t 

DT) 

00000930 

EQUIVALENCE 

(OATAt 2). 

PRNTSW) 

00000990 

EQUIVALENCE 

(DATAt 3). 

OUTSM) 

00000950 

EQUIVALENCE 

(DATA(A) i 

XFXLE) 

00000960 

EQUIVALENCE 

(DATA! 5), 

SPARED 

00000970 

EQUIVALENCE 

<DATA(6), 

SPARED 

00000960 

EQUIVALENCE 

( DATAt  7 ) i 

CO) 

00000990 

EQUIVALENCE 

(DATA! IS J 

• QBAX(D) 

00001000 

EQUIVALENCE 

(DATA! 22 ) 

iQBAY(D) 

00001010 

EQUIVALENCE 

(0ATAt3X) 

.QGAZt  1 ) ) 

00001020 

EQUIVALENCE 

(DATA (AO) 

*HRC) 

00001030 

EQUIVALENCE 

( DATAt 92) 

i QUANT) 

00001090 

EQUIVALENCE 

t DATAt A3) 

iOIAS(D) 

00001050 

EQUIVALENCE 

( OATAt  96) 

»K) 

00001060 

EQUIVALENCE 

(DATAt A7) 

.1) 

00001070 

EQUIVALENCE 

(DATAt AS) 

iDELZ) 

00001080 

EQUIVALENCE 

(OATAt  51) 

iKOtl)) 

00001090 

EQUIVALENCE 

(OATAt 59) 

iKP(D) 

oooonoo 

EQUIVALENCE 

(OATAt 57) 

*KIItll) 

Q000U10 

EQUIVALENCE 

(DATAt 60 ) 

(KPPd) ) 

00001120 

EQUIVALENCE 

(DATAt 63) 

•KXO(l)) 

00001130 

EQUIVALENCE 

(DATAt  66) 

• KIPtD) 

00011190 

EQUIVALENCE 

(DATAt 69) 

•KOPt I ) ) 

00001150 

EQUIVALENCE 

(OATAt  72) 

•BIASVtD) 

00001160 

EQUIVALENCE 

(DATAt  75) 

•SFPOtl)) 

00001170 

EQUIVALENCE 

t OATAt 75) 

.SFMO(D) 

oooonoo 

EQUIVALENCE 

t OATAt  M) 

tNODPDT) 

00001190 

EQUIVALENCE 

(OATAt 02) 

•ORDER) 

00001200 
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EQUIVALENCE  !0ATA<83) ,SFPll  1 ) ) 

EQUIVALENCE  < DATA! 86 ) .SFH1U) ) 

EQUIVALENCE  (DATA! 09 ) ,RXt 1 J ) 

EQUIVALENCE  (DATA!  92 ) ,RY!1  ) ) 

EQUIVALENCE  (DATA! 95) ,RZtl) ) 

C 

EQUIVALENCE  CPDATA(l),  ME) 

EQUIVALENCE  <PDATA<2),  RE) 

EQUIVALENCE  (PDATA<3),  G) 

EQUIVALENCE  (PDATA(9),  PRNTDT) 

EQUIVALENCE  (P0ATA15),  PBUFll)) 

C 

DATA  ABIAS  /3#0./ 

DATA  AM  /O./ 

DATA  THDQT/3*0 ./ 

DATA  THETA/3*0./ 

OATA  TFRMOD/O.O/ 

DATA  TPRNT/0 .0/ 

DATA  TTHET/S^O./ 

DATA  INITSW/O/ 

DATA  IFILE/BO/ 

DATA  ITHET/O/ 

OATA  IX/3/ 

OATA  K10/0/ 

OATA  Kll/O/ 

OATA  K12/Q/ 

DATA  PFILE/7/ 

C 

IF  (IENDF.EQ.1)  RETURN 
IF  ( INITSW.EQ.O ) GO  TO  500 
IF  (T.LT.TACC-.0001)  RETURN 
C 

K10=K10*1 
00  900  11=1,  3 

IF  Ul.GT.l)  GOTO  110 
C 

C TRANSFORM  FROM  BODY  TO  ACCELEROMETER  COORDINATES!  FROM  XYZ  TO  XOP) 
C 

C TRANSFORM  X ACCELEROMETER 
C CALCULATE  LEVER  ARM  EFFECT 
C 

CPOSSK 1 )=KBB<  Z )*RX<  3 )-KBB<  3 )*RX!  Z ) 

CR0S31 1 Z )=HD3t  3 )*RX<  X ) -Ml  3(1  )»RX(  3 ) 

CR033XI 3 )=K33( 1 )»RX< Z )-U90( 2 )»RX! 1 ) 

CROSS?!  X )=MOD(  Z )«CKOSSt<  3)-MDS(  3)*CR0SS1(  2 ) 

CROSSE!  2 m!BB<  3 )»CROSSlt  1 )-K8B(  1 )*CAOSSlt  3) 

CROSS?!  3 >=WBB!  1 HCROSSU  2 ! -W33!  2 >*CROSSl(  X ) 

CROSSX!  X )=WOOT(  2 )*RXl  3 l-WDOTl  3 )*RXt  2 ) 

CROSS!! 2 )=»WOOT( 1 ) "RX( 3 ) ♦MSOTI 3 )*RX( l > 

CROSSlf  3)=WD0T(  1 )*RX(  2 l-MOOT!  2 >#RX(  X > 

C 

00  105  12=1,3 

ABC! 12 ) s ABB! 12 1 ♦CR0352 ( 12 I*CR0SS1 1 12 ) 

105  CONTINUE 
C 

CALL  MXV! QDAX , ABC , AB A ) 

. CALL  MXV! QBAX , WCB i MBA ) 

CALL  MXV! QBAX, MOOT .NDOTAI 
GOTO  130 
C 


00001210 

00001220 

00001230 

00001290 

00001250 

00001260 

00001270 

00001283 

00001290 

00001300 

00001310 

00001320 

00001330 

00001390 

00001350 

00001360 

00001370 

00001380 

00001390 

00001900 

00301910 

00001920 

00001930 

00001990 

00001950 

00001960 

00001970 

00001930 

00001990 

00001500 

00001510 

00001520 

00001530 

00001590 

00001550 

00001560 

00001570 

00001580 

00001590 

00001600 

00001610 

00001620 

00001630 

00001690 

00001650 

00001660 

00001670 

00001680 

00001690 

00001700 

00001710 

00001720 

00001730 

00001790 

00001750 

00001760 

00001770 

00001700 

00001790 

OOOOICOO 


( 


1-31 


110  XFdl.GT.2)  GOTO  120 
C 

C TRANSFORM  Y ACCELEROMETER 
C CALCULATE  LEVER  ARM  EFFECT 
C 

CR0SS1C1 )=KBS< 2 )«RY<  3 )-UBB< 3 )«RY<  2 J 
CROSSK  2 )=-U3B<  1 )#RY<  3 KWBBf  3 1*RY(  1 > 

CROSSK  3 )=N3B<  1 >»RY<  2 )-WBS<  2 )«RY(  1 ) 

CR0SS2<  1 I=HBB(  2 )**CR0SS1<  3 )-KBB(  3 )«CROSSK  2 ) 
CROSSOt  2 )*«BB< 3 HCR3SSK  1 I'UBBI  1 >*CR0SS1C  3) 
CR03S21 3 )=!JB3(  1 )*CR0SS1!  2 )-U0B<  2 )*CROSSit 1 ) 
CROSS! ( 1 )=WDOT<  2 )«RY<  31-i:OOT<  3 )*RY<  2 ) 
CROSSK  2 I = -MOOT!  1 >»RY<  3>+WC0TI  3 !*RY<  1 > 
CROSSK  3 )=HOQT(  1 )«RY(  2 )-KOOTt  2 )«RY(  1 ) 

C 

CO  115  12:1.3 

ADC  ( 12  )=AB8<  12  WCR03S2)  12  HCROSSK 12 ) 

115  CONTINUE 

CALL  MXV ( G3A Y . ADC • ABA  1 
CALL  MXV(QSAY.NBD.WBA) 

CALL  MXV (QSAY, MOOT, UOOTA  ) 

GOTO  130 
C 

C TRANSFORM  Z ACCELEROMETER 
C CALCULATE  LEVER  ARM  EFFECT 
C 

120  CROSSK  1 ) =WB8<  2 >«RZ(  3 >-U3B(  3 )«RZ<  2 > 

CR0SS1 ( 2 )=-W33<  1 )*RZ<  3 ) ♦NBBt 3 )«RZ<  1 ) 

CR0SS1 ( 3 ) =WBB( 1 )*RZ< 2 > -K&3< 2 1 *RZ<  1 ! 

CROSSK  1 )=l.T3<  2 1KCR0SSK  3)-W03<  3H*CR0SSK2  ) 
CROSSK 2 )=NS3<  3)*CR0SSld  >-K89<  1 J«*CROSSK  3 ) 
CF35S21 3 )=WBS<  1 J^CROSSK  2 l-U3B<  2 ACROSS!)  1 ) 
CROSSK  l )=M)OTt  2 >«RZ<  3 l-KDOTI  3 >*RZI  £ ) 
CROSSK  2 }=-NOOT(  1 l**PZI  3 WKOOYf  3I«RZ(  1 ) 
CR0SS1  ( 3 )=tJDOT< 1 )»RK 2 )-UOOT( 2 t»RZU ) 

C 

00  125  12=1,3 

AOCt  12  )=ABO(  IS  )*C»0SS2d2  )tCR033ld2 ) 

125  CONTINUE 
C 

CALL  MXVt  QD A2 , ABC . AD A > 

CAU  MXV(  GDAZ.W2B.N3A  t 
CALL  MXV<  QOAZ .HOOT . U30TA ) 

C 

130  CONTINUE 

C FRINT  9C4.CRQSS2.CR0S51 
994  FORHAT  ilX,3E15.7l 
,C 

IF  <K10.NE.lt  GOTO  135 
THETAdl  t=MKC/K*A8AU  t 
NOOTAt 2)s0.00 
C 

135  CONTINUE 

IF  (KU.EQ.O)  GOTO  130 
eX=DIA3Adl» 

.C 

.c  calculate  exponentially  correlated  RANDOM  BIAS 
c 

ADI  AS)  II  )=ABIAS<  XI  t*EX*GAUSS<  AM, 61 ) 
MAs&IASl  U KADU3U1 ) 


00001810 

00001620 

00001630 

00001840 

00001650 

00001060 

00001670 

00001880 

00001600 

00001900 

00001910 

00001920 

00001930 

00001960 

00001950 

00001960 

00001970 

00001900 

00001990 

00002000 

00002010 

00002020 

00002030 

00002060 

00022050 

00002060 

00002070 

00002080 

00002090 

00002100 

00002110 

00002120 

00002130 

000021*0 

00002150 

00002160 

00002170 

00002100 

00002190 

00002200 

000022.10 

00002220 

00002230 

00002260 

00002250 

00002260 

00002270 

00002263 

00002290 

00002300 

00002310 

00002320 

00002330 

00002360 

00002350 

00002360 

00002370 

00002360 

00002390 

00002600 
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I 


f. 


WvV:;!' 


i- 

V, 

l. 


* 


c 

GOTO  139 

139  BXA:B1AS(X1) 

139  CONTINUE 
C 

C CALCULATE  THETA 
C 

QENsK*OELI»(  W3A(  3 >*W9A<  3 J-WSAl  1 )*KBAI  1 ) WABAt  3 1*MJ?C 
HI  = raC*(ABA(l>-BIA>+0ElX*KBA(3)*M>Atl)-X*WD0TA(2) 

C 

C IF  ACCELERATION  AND  ACCELERATION*^  TERHS  ARE  ZERO.  BRANCH  AROUND 
H2=0.0 

IF  1K12.NE.1)  GOTO  650 

H2  = HRC*(-KPm)«ASA(3>-KO(Il>»AEA(2> 

X -KPPl  II  )*A8A(  3 l*ABAI  3 1-KIIt  XI  )*A0A»  1 )*A0At  l )-KIP(  II  )*ABAtl ) 

X * ABAt 3 ) -KIOU1 )*A8Al 1 >*ABA( 2 l-KOPt 11 )*ABAC  2 )*ABAl 3 1 > 

650  CONTINUE 
C 

IF  1NOBOER.NE.01  GOTO  700 
C PERFORMANCE  MODEL 
C 

THN=(H1*H21/0£N 
GOTO  600 
C 

700  IF  (N050ER.NE.1 ' GOTO  750 

C FIRST  ORDER  DIFFERENTIAL  EQUATXOJWODEL 

C 

THN=THETA<  II  WOT*(  -DEN*THETA(  U »/CO 

GOTO  600 

e 

750  IF  (N0R0ER.NE.2J  GOTO  2000 
C SECOND  ORDER  DIFFERENTIAL  E0UATIONH0OEL 
C 

t«NsTHETA<  U t*OT*THOOT(Il  > 

TL’OOTI  II  l-tHOOT(  XI  )♦«  -CtWHflOTI  U >-OEH»THETAT  II  >*Hl*tt2  >*OU 
COO  CONTINUE 
■ C 

C CALCULATE  SUAMTI2EO  OUTPUT 
TTHEtmtatmUTtHEmn 
IF  TTYMETt 11  i ,LT.O.  > GOTO  190 

ti  »sPot  u i *i . ♦spit  xi  t»iHt;*K/nac 

GOTO  195 

190  Tl&SHOt II )*1. •SHlt 11 >*1HH*K/HRC 
19S  ITHETsttWETUU/AOUANT/Tl 
TTsItHfT 

TTHETF  U IstTHETI  IU-TT»U«ARUAMT 
THETAdll'ThH 
C INTEGRATE  EiV 

’ OV(  ID-OVI II  WTT*AOUANT/KflC*K*0T 

c 

C PRINT  151  • THN , THOGTT  1 1 1 . 1TH6I  *7THETI  11  > . AOAt  i > *U8At  1 >»TKETAlll  I 
151  F CROAT  llXi2E15.6.n0,/,9El5,7) 

A30  . CCHTlKUfc 
C 

C OUTPUT  AND  PRINT  CONTROL 
C 

1 IF  IPPHTOT.GlsOI  GO  TO  960 

IF  tttOaPOT.ER.OI  GO  TO  999 
C 

IP  TT.LT.TPRi1tiD-.OQOS>  GO  TO  999 


00002910 

00002920 

00002930 

00002990 

00002950 

00002960 

00002970 

00002960 

00002990 

00002500 

00002510 

00002520 

00002530 

00002590 

00002550 

00002560 

00002570 

00002500 

00002590 

00002690 

00002610 

00002620 

00002630 

00002690 

00002650 

00002660 

00002670 

00002660 

00002690 

00002700 

00002710 

00002720 

00002730 

00002790 

O0OO2V5O 

00002760 

00092770 

00002160 

00002750 

00002600 

00002610 

0000262C 

00002630 

OO09289O 

30002050 

Q6Q02C60 

.00002679 

oooozcco 

00092890 

00002900 

00902910 

00002920 

00902930 

08002990 

00002950 

00002960 

00002970 

00002960 

00002990 

00003000 
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TPRHOO =TPRMQO +MODPDT 
GO  TO  970 

960  CONTINUE 

IF  (T.LT.TPRNT-.0005)  GO  TO  999 
TPRNT=TFRNT+PRNTOT 

970  CONTINUE 

IF  (PRNTSW.LT.l. ) GO  TO  999 
WRITE(OFILE,1200)  OV 


999  CONTINUE 
TACCsT+DT 
RETURN 


0 WRITE  (OFILE.2010) 

0 FORMAT  ("ORDER  NOT  PROPERLY  SPECIFIED") 

STOP 

ACCELEROMETER  INITIALIZATION  BEGINS  HERE 


500  CONTINUE 
REWIND  IFILE 
REWIND  FFILE 

501  READ  (IFILE, 1000)  IQ.DATA(IQ) 

IF  ( SOR IFILE ) ) 502,501 

502  CONTINUE 

505  READ  ( PFILE ,1000 ) IQ.POATAf IQ) 
IF  (EOF! PFILE))  510,503 
510  CONTINUE 

REWIND  IFILE 
REWIND  PFILE 
OFILE=XFILE 


m 


DO  519  Jl=1.3 
OV( Jlt'0.0 

SPO(  Jl)sSFPOt  JIM.E-G 
UPK  J1  )=8FPi( J1  )*1  > E-6/G 
SMOt  Jl)sSFM0(  JDMl.E-6 
SNl(Jl)aSFMlUl)*a.E-6WG 
CONTINUE 

AQUANT=GUANT«NRC/K/30.48/DT 
EX=EXP(  -DT/4G ) 

NORDERsOROER 

WRITE  (OFILE.IOIO)  OT,PRNTSW,MODPDT,PRNTDT,OUTSW,OFILE. 
X ( GBAX( 12 ) , 12 J) ,9) , 

X (03AY(I£),I2=l,9),(UBAZ(I2),I2al,9) 

WRITEIOFILE ,1011 ) X, DELI, CO, MRC, QUANT 

WRITE  (OFILE.1012)  K,  XO,KP,Kir,KPP,KIO» 

X KlPiKOP,OIASiBIASV,SFPQ»SFMO»SFPliSPHl»RX»RY ,RZ,NORDER 

00  555  10® 1,3 

6 1 ASA  ( IG)aSQRTtOUSV(  X6>«(  1-EX»EX) >*1 ,E-6*G 
BIAS!  U)  i=OIAS(  16  )**1 . E-6*G 
IF  (BIA3V(I6).NE.O. ) KUal 


00003010 
00003020 
00003030 
00003040 
00003050 
00003060 
00003070 
00003060 
00003090 
00003100 
00003110 
00003120 
00003130 
00003140 
00003150 
00003160 
00003170 
00003160 
00003190 
00003200 
00003210 
00003220 
00003230 
00003240 
00003250 
00003260 
00003270 
00003280 
00003290 
00003300 
00003310 
00003320 
00003330 
00003340 
00003350 
00003360 
00003370 
00003300 
00003390 
00003400 
00003410 
00003420 
00003430 
00003440 
02003450 
00003460 
00003470 
00003400 
00003490 
00003500 
00003510 
00003520 
00003330 
00003340 
00003550 
00003560 
00003570 
. 00003500 
,00003590 
00003600 


I 


525  CONTINUE 
C 

X2=l.E-6/S 
DO  530  17=1,3 
C 


IF  (KO( 17) .NE.O » ) K12=l 
KOU7)=KO(I7)*l.E-6 
IF  (KP(I7).NE.O. J Kl£=l 
KP(I7)=KP(I7)*l.E-6 
IF  (KII(I7).NE.O.)  K12=l 
KIKI7)=KIX<I7)*X2 
IF  <KIO(I7).NE.O.)  X12=1 
Kie(I7)=KI0(I7)*(X2 
IF  (KIP( 17) .NE.O. ) K12=l 
KIP(I?)=KIP(I7)*X2 
IF  ( KdPt 17)  ,Nc ,0. ) K12=l 
K0PII7)=K0Pf  t?)#X2 
IF  (KPP( 17 ) .NE.O. 1 K12=l 
KPP(I7)=KPP(I7)*fX2 
530  CONTINUE 
C 

K9C?MRC*7.23E-S 
C0=CC*2.37E-6 
0ELI»i)ELI**2.37E-6 
X=K*  2 . 37E-6 
1=1*2. 37£*6 
0X1=0171 
C 


X-NITSHPl 

TACC=TiOT 

RETURN 

C 

C 

1060  FORMAT  US,lX,F£0.10i 

loio  format)  son  accelerometer  initialization  , 
X/3X.6N  OT  ,3X,S16.6,3X.6H  SEC, 

X/3X.3H  PRNT3U  .3X, 615.6. 

X/3X.8H  H00P6T  ,3X,(J16.6, 

X/JX.6H  PR N TOT  .3X.G16.6. 

X/3X.0H  CUTSN  ,3X,£U.6, 

X/3XV8H  OFl'LE  ,3X,I15. 

X/3VV3H  QSAX  s , JtSK.SlO.d)# 

X/9X,S(3X,G16.fl), 

X/9X,lt3X,G16.e>, 

X/3X.CM  QSAY  * .3)  3X<G16.6). 

X/9X,3t3X,G16,a>. 

xm,?)sx,si6.e), 

. X/3X.0H  nBAZ  # ,M5X, 016.0), 

X/9X,3<3X,Gl6.0h 
X/9X,3( 3X.GX6.3)) 

10U  FORMAT) 

X/3X.0M  I , 3X,C16 ,6»9H  ON  CH#*8i 

X/3N.0U  DEU  .3X.O16.0.9M  Gh  CH«»E,- 

X/SX.6H  CO  ,5>.,Glb.8»i«H  OWE  CK/RAD/SECi, 

X/3X,0H  MRS  ,JX»G16.0,0H  GH«H, 


, X/3X.SH  WANT  .3X.016.&.7M  CfVSECl 

.1012  FORMAT  (3X.6K  K .3X.G16.0.2OH  OH  CH»»2/«AD  3EC<*t*2, 


X /21X*!,X"*10X*”V,,»16X,"ZMf 

X/1X.6N  KG  .31  3X, 016.0) , ION  MICRO  G/G, 

XAJK.6H  KP  ,3)SX,G16.6S,1CH  MICRO  0/0. 


00003610 

00003620 

00003630 

00003660 

00003650 

00003660 

00003670 

00003660 

00003690 

00003700 

00003710 

00003720 

00003730 

00003760 

00003750 

00003760 

00003770 

00603760 

00003790 

00003800 

00003610 

00003620 

00003830 

00003860, 

00003850 

00003060 

00003870 

00003380 

000035.90 

00003900 

ooowno 

00003920 

00003913 

00003960 

90003950 

00003960 

00003970 

00003960 

00003990 

00906060 

00006010 

00006020 

00006030 

08006060 

00006050 

00806080 

00*06070 

00006000 

00006090 

00006100 

00006110 

00006120 

00006130 

00006160 

00006150 

00006160 

00006170 

00006160 

00006190 

00006200 
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ni-ag 


X/3X.8H  KXI  >3t 3X»G16.B) »13H  MICRO  G/G##2» 

X/3X»8H  KPP  >3( 3X,G16.8) ,13H  MICRO  G/S**2, 

X/3X.8H  KIO  i3(3X,G16.8)  >13H  MICRO  G/G**2, 

X/3X.8H  KIP  ,3<3X,G16.8),13H  MICRO  G/G**2, 

X/3X.8H  KOP  ,3(3X,G16.81,13H  MICRO  G/6«*2, 

X/3X.6H  BIAS  ,3<3X,G16.8),8H  MICRO  G» 

X/3X.SH  BIASV  ,3<3X,G16.8),13H  (MICRO  G)«*2, 
X/3X.8H  SFPO  ,3<3X,G16.8),4H  PPM, 

X/3X.6H  SFMO  ,3(3X,G16.8),4H  PPM, 

X/3X.8H  SFP1  ,3( 3X,G16.8) ,7H  PPM  /G, 

X/3X,8H  SFMX  ,3<3X,G16.S),7H  PPM  /G, 

X/3X.8H  RX  ,3<3X,616.8),4H  FT  , 

X/3X.8H  RY  ,3<3X,G16.8),4H  FT  , 

X/3X.8H  RZ  ,3<3X,G16.8),4M  FT  , 

X/3X.6H  OROER  ,15,/) 

1’iOO  F0RMAT(1H+,8X,G16.8) 

1200  F0.7MAT(6X,25H  #*  ACC  **  DV(  FT/SEC)  >3G16.8>/) 
END 


00004210 

00004220 

00004230 

00004240 

00004250 

00004260 

00004270 

00004200 

00004290 

00004300 

00004310 

00004320 

00004330 

00004340 

00004350 

00004360 

00004370 

000043S0 


s ntr-.w 


c 

C 03/10/78  DATE  OF  CURRENT  MODULE 
C 

C THE  GYRO  MODULE  SIMULATES  A SINGLE  OEGREE  OF  FREEDOM  CSDF)  RATE 
C INTEGRATING  GYRO. 

C 

SUBROUTINE  GYROS  (T,IEN0F,WB6,WD0T,ABB, 

X 0 THETA) 

C 

REAL  ABS( 3 ) 

REAL  ABG13) 

REAL  AGUANT 
REAL  A20 
REAL  8IA 
REAL  BIAS(3) 

REAL  BIASA<3) 

REAL  BIASVf  3 ) 

REAL  BIASTC 
REAL  BUF ( 17 ) 

REAL  CO 
REAL  DATA( 97) 

REAL  DELI 
REAL  OEN 
REAL  OT 

REAL  OTHETAO)  ! 

REAL  DTHETK3) 

REAL  OTHETRt  3) 

REAL  DTI 
REAL  EX 
REAL  G 
REAL  GAUSS 
REAL  GBTAS(3) 

REAL  H 
REAL  HT 
REAL  I 
REAL  K 
REAL  KK3) 

REAL  K0(3) 

REAL  KS( 3) 

REAL  KIK3) 

REAL  KS8(3) 

REAL  K10(3) 

REAL  KOS(3) 

REAL  KISIS) 

REAL  LAT 

REAL  Ml  1 

REAL  ME 

REAL  H00P0T  i 

REAL  PDUFU6) 

REAL  PDATA(fiO) 

REAL  PI 
REAL  GGBX<9) 

REAL  QGBY«9> 

REAL  QGDZ<9) 


REA-.  QUANT 
REAL  RE 
REAL  SFMOm 
REAL  SFHltS) 
REAL  SFPOvS) 
REAL  SFPK3) 


00000010 

00000020 

00000030 

00000090 

00000050 

00000060 

00000070 

OOOOOOSd 

00000090 

00000100 

00000110 

00000120 

00000130 

00000190 

00000150 

00000160 

00000170 

00000180 

00000190 

00000200 

00000210 

00000220 

00000230 

00000290 

00000250 

00000260 

00000270 

00000280 

00000290 

00000300 

00000310 

00000320 

00000330 

00000390 

00000350 

00000360 

00000370 

00000380 

00000390 

00000900 

00000910 

00000920 

00000930 

00000990 

00000950 

00000960 

00000970 

00000980 

00000990 

00000500 

00000510 

00000520 

00000530 

00000590 

00000550 

00000560 

00000570 

00000500 

00000590 

00000600 


1-37 


REAL  SM0(3) 

00000610 

REAL  SM1( 3) 

00000620 

REAL  SPOt  3 ) 

00000630 

REAL  SPlt  3) 

00000660 

REAL  SPARE1 

00000650 

REAL  SPARES 

00000660 

REAL  T 

00000670 

REAL  TOTHET 

00000680 

REAL  TGYR 

00000690 

REAL  THDOTt  3 1 

00000700 

REAL  THN 

00000710 

REAL  THETA(3) 

00000720 

REAL  TPRNT 

00000730 

REAL  TPRMOO 

00000760 

REAL  TRAHSlt  3 ) 

00000750 

REAL  TRANTC(3! 

00000760 

REAL  TT 

■ 00000770 

REAL  TTHETt  3) 

00000780 

REAL  T1 

00000790 

REAL  WBB(3) 

00000800 

REAL  WB3(  3) 

00000810 

REAL  WBDOT( 3) 

00000820 

REAL  WDOT(3) 

00000830 

REAL  ME 

00000860 

00000850 

INTEGER  IENDF 

00000860 

' INTEGER  IFILE 

00000870 

INTEGER  INITSW 

00000880 

00000890 

INTEGER  OFILE 

00000900 

INTEGER  OUTSW 

00000910 

INTEGER  PFILE 

00000920 

00000930 

EQUIVALENCE  (OATAtl), 

DT) 

00000960 

EQUIVALENCE  (DATA(2h 

PRNTSW) 

00000950 

EQUIVALENCE  (0ATA13). 

OUTSW) 

00000960 

EQUIVALENCE  (DATA) A). 

XFILE ) 

0000097C 

EQUIVALENCE  ( DATA( 5 J ► 

SPARED 

00000980 

EQUIVALENCE  (DATA(6)i 

SPARES) 

00000990 

EQUIVALENCE  (DATA! 7). 

CO) 

00001000 

EQUIVALENCE,  (0ATAU3) 

»GGBX(1)) 

00001010 

EQUIVALENCE  (DATA) 22) 

iQGBY(l)) 

00001020 

EQUIVALENCE  (DATA) 31) 

,QG3ZU)> 

00001030 

EQUIVALENCE  (DATA!  60) 

• H) 

00001060 

EQUIVALENCE  (DATA! 62 ) 

,QUANT 1 

00001050 

EQUIVALENCE  (0ATA163) 

iBIAS(D) 

00001060 

EQUIVALENCE  (DAT At  66  ) 

iK) 

00001070 

EQUIVALENCE  (DATA! *7) 

(I) 

00001080 

EQUIVALENCE  (DATA! AG) 

(DELI) 

00001090 

EQUIVALENCE  (DATA! 50) 

(Kill)) 

00001100 

EQUIVALENCE  (DATA! 53) 

iKO(l) ) 

00001110 

EQUIVALENCE  (OATA(St') 

(Kstm 

00001120 

EQUIVALENCE  (0ATA(S9) 

iKIK  1 ) ) 

00001130 

EQUIVALENCE  (DATA! 62) 

(KSSID) 

00001160 

EQUIVALENCE  (DATA! 65) 

(KIO(l)l 

00001150 

EQUIVALENCE  (DATA! 66) 

iKXS(ll) 

00001160 

EQUIVALENCE  (DATA! 71) 

,KOS(D) 

00001170 

EQUIVALENCE  (DATA! 76) 

(DIASV(l)) 

00001180 

EQUIVALENCE  (0ATA<77) 

(SFPO(D) 

00001190 

EQUIVALENCE  (0ATA(60) 

,SFMO(D) 

00001200 

1-38 


EQUIVALENCE  (0ATA(83),M0DPDT) 

00001210 

EQUIVALENCE  < DATA! 84 ) .ORDER ) 

00001220 

EQUIVALENCE  (0ATA(8S),TRANSXlin 

00001230 

EQUIVALENCE  ( 0 ATA( 88 ) . TRANTC ( 1 ) ) 

00001240 

EQUIVALENCE  (0ATA(91),SFP1(1)) 

00001250 

EQUIVALENCE  (0ATA(94),SFMl(in 

00001260 

EQUIVALENCE  (DATA(97),6IASTC> 

00001270 

c 

00001230 

EQUIVALENCE  <PDATA(lh  ME) 

00001290 

EQUIVALENCE  (PDATA(2),  RE) 

00001300 

EQUIVALENCE  (P0ATA(3),  G) 

00001310 

EQUIVALENCE  <P0ATA(4),  PRNTDT ) 

00001320 

EQUIVALENCE  (PDATA(5).  PBUF(l)) 

00001330 

c 

00001340 

OAT A AM  /O./ 

00001350 

DATA  GBIAS  /3#0./ 

00001360 

DATA  PI  /3. 1415926535897  / 

00001370 

DATA  THDOT/3*0./ 

00001380 

DATA  THETA/3*0./ 

00001390 

DATA  TPRNT/O.O/ 

00001400 

DATA  TPRMOD/O . 0/ 

00001410 

OATA  TTHET/3H0./ 

00001420 

DATA  IFILE/40/ 

00001430 

DATA  INITSW/0/ 

00001440 

DATA  ITHET  /O/ 

00001450 

DATA  IX  /S/ 

00001460 

OATA  K10  /O / 

00001470 

DATA  KU  /C/ 

00001480 

OATA  KX2  /O/ 

00001490 

OATA  OFILE/6/ 

00001500 

OATA  PFILE/7/ 

00001510 

c 

00001520 

IF  (IENDF.EQ.l)  RETURN 

00001530 

IF  (INITSM.EQ.O)  GO  TO  500 

00001540 

IF  (T.LT.TGYR-.0005)  RETURN 

00001550 

c 

00001560 

c 

00001570 

DO  400  1151,3 

00001580 

IF  (Il.GT.l)  GOTO  110 

00001590 

c 

00001600 

C TRANSFORM  FROM  BODY  TO  GYRO  COORDINATES! FROM  XYZ  TO  ZOS) 

00001610 

C TRANSFORM  X GYRO 

00001620 

C 

00001630 

CALL  MXV( QGBX . ABB . ABG ) 

00001640 

CALL  MXV( QGBX » MBB .MBG ) 

00001650 

CALL  MXVCQGBX.MOOT .WBOOT) 

00001660 

GOTO  130 

00001670 

c 

00001680 

no 

XF(Xl.ST.Z)  GOTO  120 

00001690 

c 

00001700 

c transform  y gyro 

00001710 

c 

00001720 

CALL  MXV(QGBY,ABO.ABG) 

00001730 

CALL  MXV(QGBY.MDO.WBG) 

00001740 

CALL  MXVtQGBY, MOOT, WBOOT  ) 

00001750 

GOTO  130 

00001760 

:c 

00001770 

C TRANSFORM  Z GYRO 

00101780 

C 

00001790 

120 

CALL  MXV(QGBZiABBiABG) 

00001800 

1-39 


CALL  MXV ( QGB2 * WOB . WBG ) 

CALL  HXVIQGBZ.UOOT.NSDOT  ) 

C 

130  CONTINUE 
C 

C IF  THIS  IS  THE  FIRST  PASS.  INITIALIZE  THETA 
C 

IF  (KIO.NE.O)  GOTO  135 
WBDOT!  2 )=0, 

THETA!  II  )=:WBGU)*H/K 
135  CONTINUE 
C 

C IF  THE  BIAS  VARIANCE  IS  ZERO.  BRANCH  AROUND  RANDOM  NUMBER  GENERATOR 
C 

IF  (K11.EQ.0)  GOTO  13S 
BI-BIASA! II ) 

C 

C COMPUTE  EXPONENTIALY  CORRELATED  RANDOM  BIAS 
C 

GBIAS! II )=GBIAS( II )*EX+GAUSS( AM.BI ) 

BIA=BIAS(Il)+GBIAStIl) 

GOTO  139 
C 

138  8IA=BIAS( II ) 

C 

139  CONTINUE 
C 

C CALCULATE  THETA 
C 

DEN=K*OELI#(  WBG!  3 )*WBG(  3 ) -WBG!  1 )*WBG(  1 ) ) +WBG!  3 )*H 
Ml=  H*1  WBG!  1 ) -BIA-TR  ANSI  (ID  *EXP(  -T/TRANTC!  II ) ) )+DELI*WBG<  3)*WBG 
X ( 1 )-I*WBOOT< 2 ) 


C 

C IF  THE  ACCELERATION  AND  ACCELERATION**?  TERMS  ARE  0,  BRANCH  AROUNO 
C 


M2=0.0 

IF  (K12.NE.1)  GOTO  850 

H25H*(  -KK II  )*ABG(  1 )-KS(  II  )*ABG(  3 )-KO(  I)  )* 

X ABS( 2 )-KSS(  II )*ABG(  3 )*ABG(  3 J-KIIC II )*AUU(  1 )*AB5(  1 )-KIS(  II )*ABG 
X ( 1 )*ABGt 3 >-KIO< II )*ABG( 1 )*ABG( 2 >-K0S( II )*ABG( 2 J»ABG( 3 ) ) 

850  CONTINUE 
C 

IF  (NORDER.NE.O)  GOTO  700 
C 

C CALCULATE  THETA  USING  A PERFORMANCE ;MODEL 
C , 

THN=(M1AH2)/0€N 

,C 

GOTO  800 
C 

700  IF  (NOROER.NE.l ) GOTO  750  > 

C 

C CALCULATE  THETA  USING  A FIRST  ORDER  DIFFERENTIAL  EQUATION 
C 

THN»THETA(  II ) ♦!>'('*!  «DEN*THETA(  II  WM1*M2  VCO 
C 

GOTO  800 
C 

750  IF  (N0RDER.NE.2)  GOTO  2000 


C 


00001810 

00001820 

00001830 

00001390 

00001850 

00001860 

00001870 

00001880 

00001890 

00001900 

00001910 

00001920 

00001930 

00001990 

•00001950 

00001960 

00001970 

0OOO198O 

D0001990 

00002000 

00002010 

000020!>0 

00002030 

00002090 

00002050 

00002060 

00002070 

00002080 

00002090 

00002100 

00002110 

00002120 

00002130 

00002190 

00002150 

00002160 

00002170 

00002100 

00002190 

00002200 

00002210 

00002220 

00002230 

00002290 

00002250 

00002260 

00002270 

00002280 

00002290 

00002300 

00002310 

00002320 

00002330 

00002390 

00002350 

00002360 

00002370 

00002380 

00002390 

00002900 
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C CALCULATE  THETA  USING  A SECOND  ORDER  DIFFERENTIAL  EQUATION 
C 

THN=THETA( II )+OT*THDOT( II ) 

THDQT( II )=THDOT( II )♦( -CO«THDOT(  II )-DEN»THETAt  II HM1+M2 )*DTI 
C 

800  CONTINUE 
C 

C CALCULATE  QUANTIZED  DELTA  THETA 
C 

TTHETI II  )=THN+TTHET< II ) 

IF  (TTHETdl ).  LT.O.  ) GOTO  140 
T1=1.*SP0(I1 )*SP1( II )#THN/H#K 
GOTO  145 
C 

140  T1=1.+SM0C II )*SM1( II )#THN/H#K 
C 

145  ITHET=TTHET( IX l/AQUANT/Tl 
TT=ITHET 

TTHET( II )=TTHET< IX )-TT»Tl*AQUANT 
THETA(I1)=THN 
C 

C INTEGRATE  DTHETA 

C 

C 

DTHETA( II JsDTHETAt II  >+TT*AqUANT*DT/H*K 
C PRINT  151,  THN,THD0T(I1),ITHET,  TTHETdl),  A8G(1 ) ,WBG(  l),THETAUl ) 
151  FORMAT  ( 1X,2E15.S,I10,/,4E15.7) 

400  CONTINUE 
C 

C PRINT  OUTPUT  ANO  CONTROL 
C 

IF  (PRNTDT.GT.O)  GO  TO  960 
IF  (MQDPOT.EQ.O)  GO  TO  999 
C 

IF  (T.LT.TPRMQO-.OOQ5)  GO  TO  999 
TPRMQD=TPRMOO+MGOPOT 
GO  TO  970 
C 

960  CONTINUE 

IF  ( T . LT . TPRNT - . 0005 ) GO  TO  999 
TPRNT=TPRNT+PRNTDT 

c 

970  CONTINUE 

IF  (PRNTSW.LT.l.)  GO  TO  999 
C 

WRITE! OFILE , 1200 ) DTHETA 
C 

c 

'c 

999  CONTINUE 
K10-1 
TGYR=T*OT 
RETURN 
C 
C 

2000  WRITE  (OFILE.2010) 

.2010  FORMAT  ("ORDER  NOT  PROPERLY  SPECIFIED") 

STOP 

C 

C 


00002410 

0.0002420 

00002430 

00002440 

00002450 

00002460 

00002470 

00002480 

00002490 

00002500 

00002510 

00002520 

00002530 

00002540 

00002550 

00002560 

00002570 

00002580 

00002590 

00002600 

00002610 

00002620 

00C02630 

00002640 

00002650 

00002660 

00002670 

00002680 

00002690 

00002700 

00002710 

00002720 

00002730 

00002740 

00002750 

00002760 

00002770 

00002780 

00002790 

00002800 

00002810 

00002820 

00002830 

00002840 

00002050 

00002860 

00002070 

00002880 

00002090 

00002900 

00002910 

00002920 

00002930 

00002940 

00002950 

00002960 

00002970 

00002980 

00002990 

00003000 
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o n 


READ  THE  DATA  FILES  AND  INITILIZE  THE  DATA 

500  CONTINUE 
REWIND  IFILE 
REWIND  PFILE 

501  READ  (IFILE, 1000)  IQ.DATAdQ) 

IF  (EOF(IFILE))  502,501 

502  CONTINUE 

503  READ  (PFILE.10Q0)  IQ.POATAdQ) 

IF  (EOF(PFILE) ) 510,503 

510  CONTINUE 
C 

REWIND  IFILE 
REWIND  PFILE 
OFILE=XFILE 
C 

NOROER=ORDER 

C 

WRITE  (OFILEilOlO)  DT,PRNTSW,MOOPOT,PRNTOT,OUTSW,OFILE, 
X ( GG3X( 12 ) ,12*1 ,0) , 

X ( GGBT  ( 12 ) , 12=1 , 9 ) , ( QSBZt 12 ) , 12=1 , 9 ) 

WRITEtOFILE.lOll)  QUANT 

WRITE  ( OFILE ,1012 ) K,I,OEII,CO,H,KI,KO,KS,KII 
X ,KSS,KIO,KIS  ,KOS,BIAS,BIASV,SFFO,SFMO,SFP1 
WRITE  (OFILE, 1013)  SFN1 , TRANS1 > TRANTC , BIASTC , NORDER 
C 

DO  519  Jl=l,3 
SP0(Jl)=SFP0(Jl)#l.E-6 
SPl(Jl)=SFPl(Jl)«i.E-6 
SM1( J1  /=SFM1( J1  )#1 .E>6 
SMO(  J1  )=SFMO(  J1  )**l.E-6 
519  CONTINUE 
C 

AQUANT=QUANT«PI/6 . 48E5»H/DT/K 
EX=EXP( -DT/B1A8TC) 

C 

DO  529  13=1.3 
DTHETA(I3)=0,0 

TRANSld3)=TRANSld3)#4.85£-6 
BIAGAI 13 )=SQRT(  BIASV(  13 )**(  1-EX»EX>  )»4.85E-6 
BIAS! 13 )=BIAS( I3)»4.85E-6 
IF  (BIASV!I3).NE.O. ) Kll=l 
IF ( KI ( 13 ) .NE.O, > K12=l 
KId3)=KI(X3)*4.05E-6/6 
IF(K0(I3)  .NE.O. ) K12=l 
K0U3)=K0(I3)*4.85E-6/'G 
IF(KS(I3).NE.O, ) K12=l 
KS(I3)=KStI3)*4.85E-6/'G 
* IF(  KIK  13)  .NE.  0.  ) Kl 2=1 

KIXd3)fKIId3)«A.65C-6/G/G 
IFIKSSI 13). NE.O. ) K12=1 
KSS( I3)=KS3( I3)»A.65E-6/G/G 
IF(K10(  13). NE.O. ) 1(12=1 
KI0(I3)=KI0(  I3)F4.65E-6/G/'G 
IFtKISt I3).NE.O. ) K12=l 
KISd3)=KISd3>*4,e5E«6/'GA» 

I IF(KOS(I3).NE.O. ) K12=l 

K05d3)=K0S(  131»4.85E*6/G/G 
529  CONTINUE 
C 


00003010 

00003020 

00003030 

00003090 

00003050 

00003060 

00003070 

00003080 

00003090 

00003100 

00003110 

00003120 

00003130 

00003140 

00003150 

00003160 

00003170 

00003180 

00003190 

00003200 

00003210 

00003220 

00003230 

00003240 

00003250 

00003260 

00003270 

00003280 

00003290 

00003300 

00003310 

00003320 

00003330 

00003340 

00003350 

00003360 

00003370 

00003380 

00003390 

00003400 

00003410 

00003420 

00003430 

00003440 

00003450 

00003460 

00003470 

00003480 

00003490 

00003500 

00003510 

00003520 

00003530 

00003540 

00003550 

00003560 

00003570 

00003580 

00003590 

00003600 
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C0*C0»2.37E-6 

0ELI=DELI#2.37E-6 

K=K*2.37E-6 

1*1*2. 37E-6 

DTI=DT/I 

H*H*2.37E-6 

INITSW*1 

TCYR=T+OT 

RETURN 


1000  FORMAT  < I5.1X.F20.10 ) 

1010  FORMATION  GYROSCOPE  INITIALIZATION  , 

X/3X.8H  DT  ,3X,G16.8,3X,4H  SEC. 

X/3X.8H  PRNTSW  ,3X,G16.8, 

X/3X.8H  MODPDT  ,3X,G16.8, 

X/3X.8H  PnNTDT  .3X.G16.8, 

X/3X.8H  OUTSW  ,3X,G16.8, 

X/3X.8H  OFILE  ,3X,I1S.//, 

X/3X.8H  QGBX  = ,3C3X,G16.8), 

X/10X.3t  3X>G16.8)  i 
X/10X.31 3XiG16.8) »/> 

X/3X.8H  QGBY  = ,3(3X,G16.8>, 

X/10X.31 3X.G16.8) . 

X/10X>3( 3X.G16.8) >/ . 

X/3X.8H  QGBZ  = , 3( 3X.G16.8) . 

X/10X.3! 3X.G18.8). 

X/10X,3( 3X.G16.8) ./) 

1011  FORMAT t 

X/3X.8H  qUANT  ,3X,G16.8,7H  ARCSEC,//) 

1012  FORMAT  (3X.8H  K ,3X,G16.8,16H  GM  CM*»2/SEC**2 , 

X/3X.8H  I , 3X.G16.8.9H  GH  CM**2, 

X/jX.BH  DELI  .3X.G16.8.9H  GM  CM**2, 

X/3X.6H  CO  .3X.G16.8.16H  DYNE  CM/RAD/SEC. 

X/3X.8H  H ,3X,G16.8,13H  GM  CM**2/SEC,//, 


X/21X , “X."  > 18X . "Y" . 18X  i "Z" , 

X/3X.8H  KI  ,3! 3X.G16.8) >9H  DEG/HR/G, 

XX3X.8H  KO  ,3(3X,G16.8).9H  DEG/HR/G, 

X/3X.SH  KS  ,3<3X,G16.8),9H  DEG/HR/G, 

X/3X.8H  KII  ,3(3X,G16.8),16H  DEG/HR/-G**2, 
X/3X.8H  KSS  i3(3X.G16.8)*16H  OEG/WG**2> 
X/3X.OH  KIO  *3( 3X»G16.8)il6H  DEG/HR/G**2, 
X/3X.8H  KIS  »3( 3X>G16.8) 1I6H  OEG/HR/G**2, 
X/3X.8H  KOS  ,3t3X.G16.8).16H  0EG/HR/G**2, 
X/3X.8H  BIAS  ,3(3X,G16.8),7H  DEG/HR, 

X/3X.8H  BIA3V  ,31 3X.G16.8) ,12H  1 0 EG/HR  1**2, 
X/3X.8H  SFPO  .31 3X|G16.8),4H  PPM, 

' X/3X.0H  SFMO  ,3!3XiG16.8),4H  PFM, 

X/3X.8H  SFP1  ,3( JX.G16.0J.13H  PPM/RAD/SEC  1 
1013  FORMAT! 3X.6H  SFM1  .3!3XtG16.8),13H  PPM/RAD/SEC 
X/3X.0H  TRAMS1  ,3t3X,G16.8),7H  DEG/HR, 

X/3X,8H  TRANTC  ,*<3X,G16.8),4H  SEC,/. 

X/3X.8H  BIA9TC  , 3X.G16.8  ,4H  SEC, 

X/3X,fiH  ORDER  ,3X,17,/1 
1100  FORMAT! 1H*,BX ,616. 61 

I 1200  FORMAT! 6X.25H  **  GYR  **  OTHETA(RAOS)  ,3016. 6,/) 
END 


00003610 

00003620 

00003630 

00003640 

00003650 

00003660 

00003670 

00003680 

00003690 

00003700 

00003710 

00003720 

00003730 

00003740 

00003750 

00003760 

00903770 

00003780 

00003790 

00003800 

00003810 

00003820 

00003830 

00003840 

00003850 

00003860 

00003870 

00003880 

00003890 

00003900 

00003910 

00003920 

00003930 

00003940 

000039S0 

00003960 

00003970 

00003980 

00003990 

00004000 

00004010 

00004020 

00004030 

00004040 

00004050 

00004060 

00004070 

00004080 

00004090 

00004100 

00004110 

00004120 

00004130 

00004140 

00004150 

00004160 

00004170 

00004180 
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3/15/76  DATE  OF  CURRENT  MODULE 
10/16/77  CHANGE  FORMATS  TO  G16.8 

»*««#  ###  THIS  MODULE  SIMULATES  THREE  RING  USER  GYROS  «## 

02/03/76  DATE  CF  CURRENT  MODULE.  DGR 

SUBROUTINE  GYROS  ( T . I END  F , WBB  * WOOT » ABB . 

OTHETA  ) 

•WSB  - ANGULAR  RATE  OF  BODY  IN  BODY  COORDINATES. 

.DTHETA  - INCREMENTAL  ROTATION  GENERATED  BY  GYROS. 

.WDOT i ABB  - NOT  USED. 

IMPLICIT  REAL  ( A-Z  ) 

REAL  GAUSS 

INTEGER  I>IENDF,INITSW,IFILE,IQ,J,K,OFILE.NP,PFILE 
DIMENSION  ABBt  3 ) . ANG( 3 ) , ANGRWt 3 ) , ANGWNt 3 ) ,DATA( 34 ) .DTHETAt  3 ) , 
. D3( 3) .DTAt  3) >DTC(  3) .EXPDt 3) .EXPSFt  3) .KW(  9).SF(3)»SFTA(3)i 

. SFTC( 3 ) ,WBB! 3 > ,W<  3 ) ,WOOT(  3 ) .PBUFt 16 ) ,PDATA(  20 ) 


00000010 

00000020 

00000030 

00000040 

00000050 

*###00000060 

00000070 

00000080 

00000090 

OOQOOIOO 

00000110 

00000120 

00000130 

00000140 

G0000150 

00000160 

00000170 

00000180 

00000190 

00000200 

00000210 

00000220 

00000230 


EQUIVALENCE 

( DATAC 1 ) , 

DT) 

00000240 

EQUIVALENCE 

( D ATA ( 2 ) > 

PRNTSW ) 

00000250 

EQUIVALENCE 

( DATAC  3 ) » 

OUTSW ) 

00000260 

EQUIVALENCE 

<DATA<4), 

XFILE ) 

00000270 

EQUIVALENCE 

<0ATA(5), 

SPARED 

00000280 

EQUIVALENCE 

( DATAt 6 ) » 

SPARES > 

00000290 

EQUIVALENCE 

(DATA! 7), 

MOO  POT) 

00000300 

EQUIVALENCE 

(DATA!  8), 

DDt  1 ) ) 

00000310 

EQUIVALENCE 

(DATA!  11) 

, DTA(l)) 

00000320 

EQUIVALENCE 

(DATAt  14! 

, OTC(l)) 

00000330 

EQUIVALENCE 

(DATAt 17) 

, SFTAI 1 ) ) 

00000340 

EQUIVALENCE 

(DATAt  20 ) 

, SFTCt 1 ) ) 

00000350 

EQUIVALENCE 

(DATAt 23) 

i KWt  1 ) ) 

00000360 

EQUIVALENCE 

(DATAt 32) 

, STOUN) 

00000370 

EQUIVALENCE 

(DATAt 33) 

, STCRW) 

00000380 

EQUIVALENCE 

(DATAt 34) 

> Q) 

00000390 

00000400 

EQUIVALENCE 

( PD ATA ( l > 

. WE) 

00000410 

EQUIVALENCE 

( POATAt  2 ) 

.RE) 

00000420 

EQUIVALENCE 

( PDATAt  3) 

. G) 

00000430 

EQUIVALENCE 

(POATAt 4) 

. PRNTOT) 

00000440 

EQUIVALENCE 

(PDATAt 5) 

, POUr(D) 

00000450 

DATA  DTR  /. 01745329251994330/ 
DATA  INITSW  /0/ 

DATA  IFILE  /4Q/ 

DATA  OFILE  /6/ 

OATA  ANG  /3M0./ 

DATA  AN3RM  /3«0./ 

DATA  PF1LE  /7/ 

OATA  TPRNT  /O.O/ 

OATA  TPRMOD  /O.O/ 

IF  (XEH0F.EQ.1)  RETURN 
IF  (INITSW. EQ.0>  GO  TO  500 
IF  (T.LT.TGYR-.0001)  RETURN 


00000460 

00000470 

00000480 

00000490 

00000500 

00000510 

00000520 

00000530 

00000540 

00000550 

00000560 

00000570 

00000580 

00000590 

00000600 
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c BEGIN  NORMAL  COMPUTATIONS 

C##TRANSIENT  DRIFT  ANO  TRANSIENT  SCALE  FACTOR:  OTA.SFTA. 

100  00  105  I = 1,3 

IF  ( ABS(  OTA(I)  ) .LT.  l.E-10  ) DTAd)  = 0. 

IF  ( ABSt  SFTA(I)  ) .LT.  l.E-10  ) SFTACI)  s 0. 

DTA(I)  = DTAd)»EXPD(I> 

SFTAd)  * SFTA( I )*EXPSFd ) 

Ks4#(I-l)+l 

KW(K)  = SF(  I ) + SFTAd) 

105  CONTINUE 

C##COMPUTE  WHITE  AND  RANDOM  WALK  ANGLE  NOISE:  ANGWNtANGRH. 

00  110  I = 1,3 

ANGWNd)  s STDWN#GAUSS 10. 0,1.0) 

110  AHGRW(I)  = KRU*GAUSS( 0 .0,1.0 ) 

C***COMPUTE  THE  CONTINUOUS  ANGLE:  ANG. 

CALL  MXV  ( KW.WBB.W  ) 

00  112  I = 1,3 

U(I)  = W'BBd)  + U(I)  4 DB(  I ) ♦ OTA 1 1 ) 

ANG(I)  = ANG(I)  ♦ Wd)#DT 

112  ANG(I)  = ANG!  I ) ♦ ANGWNd)  ♦ ANGRWtl) 

C*»eOMPUTE  QUANTIZED  INDICATED ' ROTATION:  OTHETA. 

C .MP  = NUMBER  OF  OUTPUT  PULSES. 

C .Q  = ANGULAR  QUANTIZATION. 

C NOTE:  THE  NAV  ROUTINES  SETS  (OTHETA)  TO  ZERO  EACH  TIME  THE 
C PLATFORM  ATTITUDE  MATRIX  IS  COHPUTED. 

DO  115  I s 1,3 

IF  (Q  .GT.  0. ) GO  TO  113 

QANS  = ANG! I ) 

GO  TO  114 

113  NP  = ANGID/Q 
QANG  = NP*Q 

114  ANG!  I ) = ANG!  I)  • QANG  - ANGWNd) 

115  DTHETAd)  - OTHETA(I)  ♦ QANG 
C 

C OUTPUT  ANO  PRINT  CONTROL 
C 

IF  (PRNTDT.GT.O)  GO  TO  960 
IF  (MODPDT.EQ.O)  GO  TO  999 
C 

IF  ( T . LT . TFRMOD- . 0005 ) GO  TO  999 

TPRMOOcTPRMOOaMCOPOT 

GO  TO  970 

c 

960  CONTINUE 

IF  ( T . LT . TPRNT - . 0005 ) GO  TO  999 
TPRNTaTPRNT*PRNTOT 

c 

970  CONTINUE 

IF  (FRNTSW.LT.l)  GO  TO  999 
C 

WRITE! CHILE, 1200)  OTHETA 
C 

999  CONTINUE 
TGVRbT*DT 
RETURN 


.C 

.C INITIALIZATION  SECTION—— — 

CttfREAO  THE  INPUT  DATA  FILE:  IFXLE . 

C .0T  - SIMULATION  TIME  STEP  i SEC  ) 

C .OS  BIAS  DRIFT  (OEQ/HR) 


>00000610 
00000620 
00000630 
00000640 
00000650 
00000660 
00000670 
00000680 
00000690 
00000700 
00000710 
00000720 
00000730 
00000740 
00000750 
00000760 
00000770 
oor 00780 
00000790 
00000800 
oooooaio 
00000820 
00000830 
00000840 
00000850 
00000860 
00000870 
00000880 
00000390 
00000900 
00000910 
00000920 
00000930 
00000940 
00000950 
00000960 
00000970 
00000980 
00000990 
00001000 
00001010 
00001020 
00001030 
00001040 
00001050 
00001060 
00001070 
00001080 
00001090 

oooeuoo 

oooomo 

00001120 

00001130 

00001140 

oooomo 
00001  HO 
-OOOOIV'9 
OOOO.Ur,1; 
OOOHvfi 
000)1200 


1 
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c 

.OTA  - DRIFT  TRANSIENT  AMPLITUDE 

(DEG/HR) 

00001210 

c 

,DTC  - DRIFT  TRANSIENT  TIME  CONSTANT 

( 

MIN 

) 

00001220 

c 

.SFTA  - SCALE  FACTOR  TRANSIENT  AMPLITUDE 

( 

PPM 

) 

00001230 

c 

,SFTC  - SCALE  FACTOR  TRANSIENT  TIME  CONSTANT 

j 

MIN 

) 

00001240 

c 

.KM  - SF  ANO  GYRO  IA  MISALIGNMENT  MATRIX 

( 

PPM 

) 

00001250 

c 

.STDWN  - STD  WHITE  ANGLE  NOISE 

( 

SEC 

) 

00001260 

c 

•STORM  - RANDOM  WALK  ANGLE  MAGNITUDE 

DEG/SQRTtHR >00001270 

c 

.0  - ANGULAR  QUANTIZATION 

C 

SEC 

) 

00001260 

c 

00001290 

c 

00001300 

500 

CONTINUE 

00001310 

REWINO  IFILE 

00001320 

REWINO  PFILE 

00001330 

501 

READ  (IFILE. 1000)  IQ.OATA(IQ) 

00001340 

IF  (EOF! IFILE))  502.501 

00001350 

502 

CONTINUE 

00001360 

503  READ  t PFILE .1000 > IQ.PDATAtlQ) 

00001370 

IF  (EOF! PFILE))  510.503 

' 

00001360 

510  CONTINUE 

00001390 

C 

00001400 

REWIND  IFILE 

00001410 

REWIND  PFILE 

-90001420 

OFILE=XFILE 

00001430 

c 

00001440 

WRITE  (OFILE.101Q)  OT.PRNTSW .PRNTOT.OUTSW.OFILE.MOOPOT 

00001450 

WRITE(OFILe.lOU)  q.stown, storm 

00001460 

WRITE  (OFILE.1012)  D3,DTA,0TC,SFTA.SFTC,KW 

00001470 

c 

00001460 

C#*1  SCALE  THE  INPUT  DATA  TO  INTERNAL  PROGRAM  UNITS. 

00001490 

0 

CO  10  I = 1,3 

00001500 

CCXI)  = 0B< 1 1*010/3600. 

00001510 

Of At  I)  = DTA( I )'<QTR/'3600. 

00001520 

OTC(I)  = DTC(  I )**t>0. 

00001530 

SFTA! I)  a 5FTAI I )*1.£«06 

00001540 

SFTC(I)  = SFTCt I)*oO. 

00001550 

DO  10  J = 1*3 

00001560 

K33*NI-1)*J 

00001570 

KU(K)  = KHtK)*l.E>06 

ooooisao 

10 

CONTINUE 

00001590 

STDWN  * ST0HN*0TR/3600. 

00091600 

KKW  a ( STORWOTH  )#SQBT(  DT/3600.  ) 

00001610 

Q * Q*OTR/3600« 

00001620 

c 

00001630 

DO  11  I a 1.3 

00001640 

K=4M  »-!)♦! 

00001650 

SFt X ) > KWtKI 

00001660 

EXPOU)  a 0. 

00001670 

EXPSF(I)  a 0, 

00001660 

• 

IF  < OTCtI)  .CT.  0.)  IXPOtll  s EXP(*OT/OTCU)  ) 

00001690 

IF  ( SFTC(I)  .GT.  0.)  EXPSF(I)  * EXP(  ‘•0T/SFTC1 X ) » 

00001700 

n 

CONTINUE 

oofomo 

c 

00001720 

XNXTSW»1 

OOOOi.^O 

T5VfiaT*0T 

00001740 

RETURN 

00001750 

L*»END  INITIALIZATION. 

00001760 

.c 

00001770 

c 

00001760 

c 

00001790 

1000  FORMAT  1X5**20.10) 

ooooieoo 

I 
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10X0  FORMAT!  30H  USER  GYRO  INITIALIZATION  . 
X/3X.8H  DT  ,3X,G16.8,3X,AH  SEC. 

X/3X.SH  PRNTSW  ,3X,G16.«, 

. X/3X.6H  PRNTDT  ,3X,G16.8, 

X/3X.3H  OUTSM  , 3X.G16.fi, 

X/3X.8H  OFILE  .3X.I7, 

X/3X.8H  MOO POT  ,3X,G16.6,3X,AH  SEC.//) 

1011  FORMAT! 

X/3X.8H  Q ,3X, G16.fi, AN  SEC. 

X/3X.0H  STDWN  .3X, G16.fi. AH  SEC. 

X/3X.SH  STORM  .3X.G16.fi.13H  CEG/SQRT1HR),//) 

1012  FORMAT  ( 


X/21X . MX“ . lfiX.^Y" , lfiX , "Z“ . 

X/JX.flH  OB  .313X.G16.fi), ?H  OEG/HR, 

X/3X.8H  DTA  .3!3X,G16.fi).7H  DEG/HB, 

X/3X.6H  OTC  .3! 3X.G16.fi), AH  MIN, 

X/3X,flH  SFTA  ,3! 3X.G16.fi), AH  PPM, 

X/3X.8H  SFTC  ,3! 3X.G16.fi), AM  MIN, 

X//3X.8M  KU=  ,3! 3X.G16.fi), AH  PPM, 

X/11X,  3!  3X.G16.fi), 

X/UX.  3!  3X,G16.fi),//) 

1200  FQRflAT(6X»25H  «»  GYR  «*  OTHETA(RAOS)  .313X.G20.12)) 
END 


00001810 

00001820 

00001Q30 

OOOOlfiAO 

00001850 

00001660 

00001670 

ooooieeo 

00001890 

00001900 

00001910 

00001920 

00001930 

OOOOI9AO 

00001950 

00001960 

00001970 

00001980 

00001990 

00002000 

00002010 

00002020 

00002030 


,V-  --m 


c 

C 03/10/76  DATE  OF  CURRENT  MODULE 
C 

C THE  ALTIMETER  MODULE  SIMULATES  A BAROMETRIC  ALTIMETER. 
C 

SUSROUTINS  ALTI  (T.IENDF»ALT,V, 

X ALTO) 

C 

REAL  ALT 
REAL  ALTA2 
REAL  ALTO 
REAL  AM 
REAL  OATAU8) 

REAL  OT 
REAL  EX 
REAL  E£Xl 
REAL  GAUSS 
REAL  MODPOT 
REAL  PBUP( 16 ) 

REAL  PDATAt 20) 

REAL  PRNTDT 
REAL  TPRMOD 
REAL  TPRNT 
REAL  V(J) 

REAL  VI2 
REAL  X 
C 

INTEGER  IENDF 
INTEGER  INITSW 
INTEGER  OUTSM 
INTEGER  IF  I LE 
INTEGER  PFILE 
INTEGER  OFILE 
C 

EQUIVALENCE  (DATA! 1 1 » OT) 

EQUIVALENCE  (DATAt?.)*  PRNTSW) 

EQUIVALENCE  (OATA(J),  OUTSW) 

EQUIVALENCE  tDATA(4)>  XFILE) 

EQUIVALENCE  (DATAtSb  MODPDT) 

EQUIVALENCE  (0ATA(6),  NOIS3W) 

EQUIVALENCE  (0ATA<7)iTC> 

EQUIVALENCE  (OATA(O).UO) 

EQUIVALENCE  (DATA(9),U1) 

EQUIVALENCE  (DATAt  10  ),U2) 

EQUIVALENCE  (DATAUD.UJ) 

EQUIVALENCE  (DATA( IS), U4> 

C 

EQUIVALENCE  (POATA(l),  LIE) 

EQUIVALENCE  ( PDATAt 2 ) , RE) 

EQUIVALENCE  (PDATAt 3),  G) 

EQUIVALENCE  (PDATAt A)*  PRNTDT) 

EQUIVALENCE  (PDATAt 5 )»  PfiUF(D) 

C 

DATA  AM/0,0/ 

DATA  TPRMOO/O.O/ 

DATA  TPRNT/0.0/ 

, DATA  X/0,0/ 


000000X0 

00000020 

00000030 

00000040 

00000050 

00000060 

00000070 

00000060 

00000090 

00000100 

ooooono 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

00000160 

00000190 

00000200 

00000210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 

00000200 

00000290 

00000300 

00000310 

00000320 

00000330 

00000340 

000~0350 

00000360 

00000370 

00000360 

00000390 

00000400 

00000410 

00000420 

00000430 

00000440 

00000950 

00000460 

00000470 

00000400 

00000490 

00000500 

00000510 

00000520 

00000530 

00000540 

00000550 

00000560 

00000570 

00000500 

00000590 

00000600 


c 


I 

§ 

1 


I 

i 

) 


i 

■j 

% 


% 

\ 

J. 

i 

* 

I 


i 

i 

i 


i 


i 


S' 


I 


{\ 


fc 


IF  (IEN0F.EQ.1)  RETURN 
IF  (INITSW.EQ.O)  GO  TO  500 
IF  (T.LT.TALT-.OOQ2)  RETURN 
C 

ALTO=ALT 

C 

C AOO  NOISE  INTO  ALTITUDE  COMPUTATION  IF  N0ISSW=1 
C 

IF  (NOISSU.LT. I. ) GO  TO  110 
C 

c 

ALTA2=ALT**2 

VI2=V(  1 )«V(  1 )*V(  2 )#V<  2 )+V( 3)*VC  3) 

SIGN=SQRT< (U0*ALTA2»ALTA2t 
X U1*VI2»VX2*U2 1*12X1) 

SIGR=SQRT(  U3*ALTA2*U4 ) 

C 

C CALCULATE  EXPONENTIALLY  CORRELATEONQISE  TERM 
C 

X=EX*X+GAUSS(AH,SIGN) 

C 

C CALCULATE  BAROMETRIC  ALTITUDE 
C 

ALTO=ALT*X+GAUSS( AMiSIGR > 

C 

110  CONTINUE 

C 

C OUTPUT  AND  PRINT  CONTROL 
C 

IF  l PRNTOT.GT.O)  GO  TO  960 
IF  IM30P0T.EQ.0)  GO  TO  999 
C 

IP  (T.LY.TPRNOD-.OOOS)  GO  TO  999 
TPRUOD  sTPRMOD+HOOPOT 
GO  TO  970 
C 

960  CONTINUE 

IF  ( T . LT . TPRNT - . 0005 ) GO  TO  999 
TPRNTbTP«NT*PRNTOT 
C 

970  CONTINUE 

IF  i PRNTSM.LT. I. ) GO  TO  999 
URITEIOFILE, 12001  ALTO 
L 

999 'CONTINUE 
TALT=T«0T 

. return 

c 

C ALTIMETER  INITIALIIATiON 

c 

500  CONTINUE 
C 

REWIND  IFIIC 
REWIND  PFILE 

501  READ  UFKE.iODOl  IX, DATA! XX) 

. IF  (EOFIIFILEJ)  502,501 

502  CONTINUE 

C ' 

503  READ  (PFILE, 1900)  XX.POATAUX) 


09000610 

00000620 

00000630 

00000640 

00000650 

00000660 

00000670 

00000680 

00000690 

00000700 

00000710 

00000720 

00000730 

00000740 

00000750 

00000760 

00600770 

00000780 

00000790 

Q0000800 

00000810 

00000820 

00000830 

00000840 

00000850 

00000860 

00000870 

00000880 

00000&90 

00000900 

00000910 

00000920 

00000930 

00000940 

00000950 

00000960 

00000970 

00000980 

00000990 

00001000 

00001010 

00001020 

00001030 

00001040 

00001050 

00001060 

00001070 

00001090 

00001090 

00001100 

ooooxno 

00001120 

00001130 

00001140 

00001150 

00001160 

00001170 

00001180 

OOP0U90 

OOOOlStN) 
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IF  t EOF* PFIl.E > > 510,503 

510  CONTINUE 
C 

REMIND  PFILE 

REMIND  IFILE 
C 

ALTO=ALT 

C 

EX=EXP( -DT/TC) 

£2X1=1-EX«#2 

INITIALIZATION  OUTPUT  AND  PRINT  CONTROL 
OFILE=XFILE 

WRITE  (OFILEilOlO)  OT,PRNTSM,OUTSM,OFILE,PRNTDT,MODPDT,NOISSW 
WRITE  (0FILE.1012)  TC,U0,U1,U2»U3,U4 

INITSMsl 
TALTsT*OT 
RETURN 

END  OF  INITIALIZATION 
1C00  FORMAT  U5.F20.10) 

1010  FORMAT  ( 30H  ALTIMETER  INITIALIZATION  , 

X/3X.6H  OT  ,3X,G16.8,3X,4H  SEC, 

. X/3X.6H  PRNTSW  ,3X,616.6, 

X/3X.8H  CUTSW  ,3X,G16.8, 

X/'iXtSH  OFILE  ,3X,I15, 

X/3X.6H  PRNTDT  .3X.616.8, 

X/3X.8H  MODPOT  ,3X,G16.8» 

X/3X.8H  NOISSM  ,3X,X15,//J 
1012  FORMAT<  3X , 30H  ALTIMETER  UNCERTAINTIES  ,/, 

X/6X,i4H  TC  ,3X,G16.8, 

X/6X.14M  UOIFT-2)  ,3X,G16.8, 

X/6X,14H  UKSECVFT2)  ,3X, 616.0, 

X/6XaAH  U21FT2)  ,3X,G16.8, 

X/6X.14H  U3  , 3X.G16 .8, 

X/6X.14H  URJ  FTC  l ,3X, 616.8,//) 

1200  F0t'1t1ATl6X»C5H  »*  ALT  ALT  (FT)  ,616.8,//) 

• END  ' : . 


1-50 


00001210 

00001220 

00001230 

00001240 

00001250 

00001260 

00001270 

00001280 

00001290 

00001300 

00001310 

00001320 

00001330 

00001340 

00001350 

00001360 

00001370 

00001380 

00001390 

00001400 

00001410 

00001420 

00001450 

00001440 

00001450 

00001460 

00001470 

00001480 

00001490 

00001500 

00001510 

00001520 

00001530 

00001540 

00001550 

00001560 

00001570 

00001500 

00001590 

00001600 

00001610 
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03/10/73  DATE  OF  CURRENT  MODULE 


THE  HAROMARE/SOFTHARE  INTERFACE  MODULE 
THE  INSS  SEQUENCE  FAST  AND  SLOW  CYCLES 


C 

c 


SUBROUTINE  RDR  (T,IENDF,DTHETA,DV, 
X OTHETO.OVO) 


REAL  DATA!  6) 
REAL  OTHETAt  3) 
REAL  DTHETO(3) 
REAL  DV(3) 

REAL  DV0(3) 
REAL  MQDPQT 
REAL  SPARE1 
REAL  PBUFt 16 ) 
REAL  PDATAt  20 ) 
REAL  PRNTDT 
REAL  T 
REAL  TPRNT 
REAL  TPRMOD 
REAL  TEMP( 3) 
REAL  TRDR 


INTEGER  IENDF 
INTEGER  OFILE 
INTEGER  PFILE 


EQUIVALENCE  (DATAtl),  OT1 
EQUIVALENCE  (DATA(2),  PRNTSW) 
EQUIVALENCE  <0ATA<3>.  OUTSW) 
EQUIVALENCE  (0ATAt4),  XF1LE ) 
EQUIVALENCE  (OATA(S),  SPARED 
EQUIVALENCE  (0ATAI6),  MOOPOT) 


EQUIVALENCE  ( PDATAt 1).  WE) 
EQUIVALENCE  (PDATAt 2),  RE) 
EQUIVALENCE  (PDATAt  3),  G) 
EQUIVALENCE  (PDATAt A),  PRNTDT) 
EQUIVALENCE  (PDATAt 5).  PBUFt D) 


DATA  TPRNT/O.O/ 
DATA  TPRMOD/O.O/ 
DATA  INITSW/O/ 
OATA  1FILE/65/  ■ 
DATA  PPILE/7/ 


IF  UENOF.EQ.l  ) RETURN 
IFtINITSW.EQ.O)  GOTO  SOO 
iFtT.LT. TRDR*, 0001)  RETURN 


DO  100  1=1,3 

ovcmsQvm 

OVtDPO. 

OTHETOt  I ) aDTHETAt I ) 
DTHETAtDaO. 

100  CONTINUE 


OOOOOOIO 

00000020 

00000030 

FUNCTIONS  AS  A BUFFER  BETWEEN  00000040 

00000050 
00000060 
OOf 00070 
0C000080 
00000090 
00000100 
00000110 
00000120 
00000130 
00000140 
00000150 
00000160 
00000170 
00000160 
P0000190 
00000200 
00000210 
00000220 
00000230 
00000240 
00000250 
00000260 
00000270 
00000260 
00000290 
00000300 
00000310 
00000320 
00000330 
00000340 
00000350 
00000360 
00000370 
00000360 
00000390 
00000400 
00000410 
00000420 
00000430 
00000440 
00000450 
00000460 
00000470 
00000460 
00000490 
OOOOOSOO 
00000510 
00000320 
00000530 
00000540 
00000550 
00000560 
OQCOQS70 
00000560 
00000590 
00000600 
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c 

C OUTPUT  AND  PRINT  CONTROL 
C 

IF  (PRNTDT.G7.0)  GO  TO  960 
IF  (MODPDT.EQ.O)  GO  TO  999 
C 

IF  (T.LT.TPRMOD-.OOOS)  GO  TO  999 
TPRHOD=TPRMOD*MOOPDT 
CO  TO  970 
C 

960  CONTINUE 

IF  (T.LT.TPRNT-.0005)  GO  TO  999 
TPRMTaTPRNT +PRNT0T 
C 

970  CONTINUE 

IF  (FRNTSW.LT.l)  GO  TO  999 
WRITE (OFILE ,1200  JDVOiDTNETO 
C 
C 

999  CONTINUE 

TRDRsTKOR+DT 

RETURN 

C 

C READER  INITIALIZATION  BEGINS  HERE 
C 

500  CONTINUE 
REWIND  IFILE 

501  READ  (IFILE, 1000  )IX,DATA(  IX) 

IF  (EOF( IFILE))  502,501 

502  CONTINUE 

503  READ! PFILE ,1000 ) IX.POATA(IX) 

IF  ( EOF t PFILE ) ) 510,503 

510  CONTINUE 
C 

REWIND  IFILE 
REWIND  PFILE 
OfILE=XFILE 
C 

WRITE!  OFILE 1 1010 )DT#PRNT3U,OUTSM»OFXLE  >HODPDT»PRNTOT 
C 

INITSW=1 

TRDR=T*DT 

RETURN 

C 

C END  OF  INITIALIZATION 
C 

1000  FORMAT( I5,F£Q . 10 ) 

1010  FOR(1AT(30H  READER  INITIALIZATION  , 

X/3X.6H  DT  i3X,G16.6i3X»9H  SEC, 

X/3X.6H  PRNTSW  ,3X,G16.0, 

XX3X.8H  OUTSW  ,3X,G16.0, 

X/JXiOH  OFILE  ,3X,U5, 

X/3X.0H  KODPDT  ,3X,G16.fl, 

X73X.8H  PRNTDT  ,3X,G16.8,yV ) 

1200  FORMAT(6X,25H  **  ROR  OV(FVSEC)  ,3G16.»,/ 

, X 6X.2SH  OTMETA(RAOS)  ,3016. 6, /> 

. END 


0000061b 

00000620 

00000630 

00000690 

00000650 

00000660 

00000670 

00000660 

00000690 

00000700 

00000710 

00000720 

00000730 

00000790 

00000750 

00000760 

00000770 

00000700 

00000790 

00000600 

00000810 

00000320 

00000830 

00000690 

00000850 

00000860 

00000370 

00000680 

00000090 

00000900 

oooqoqio 

00000920 

00000930 

00000990 

00000950 

00000960 

00000970 

00000900 

00000990 

ooroiooo 

00001010 

00001020 

00001030 

00001090 

00001050 

00001060 

00001070 

00001060 

0000X090 

00001100 

oooomo 

00001120 

00001150 

00001190 

00001150 

00001160 

00001170 


03/10/76  DATE  OF  CURRENT  MODULE 

THE  ACCELEROMETER  COMPENSATION  MODULE  COMPENSATES  FOR  ACCELEROMETER 

DIASES  AND  ERRORS. 

SUBROUTINE  ACOMP  (T,IENOF,OVO»DTHETA. 

X OVA) 

REAL  BIAS! 3) 

REAL  CROSSK3) 

REAL  CROSS2( 3) 

REAL  CR0SS3U) 

REAL  DATA173) 

REAL  DT 
REAL  0DTHC3) 

REAL  DELI 
REAL  OTHETAI 3) 

REAL  DTHETP(3) 

REAL  DTHETZ(3) 

REAL  DVA( 3 ) 

REAL  DV0( 3 ) 

REAL  8 
REAL  IXX 
REAL  KIK3) 

REAL  MRC 
REAL  MODPDT 
REAL  PBUFU6) 

REAL  PDATAt  20 ) 

REAL  QAB0(9> 

REAL  QABX19) 

REAL  QABY ( 9 ) 

REAL  QABZ<9> 

REAL  QMISt  9) 

REAL  RX(3) 

REAL  RY13) 

REAL  RZ(3) 

REAL  SFMO(S) 

REAL  SFMlt  3) 

REAL  SFP0(3> 

REAL  SFPN3) 

REAL  SM0( 3) 

REAL  SMH3) 

REAL  SP0( 3) 

REAL  SPII3) 

REAL  SPARE1 
REAL  SPARES 
REAL  TFRMuD 
REAL  TPSNT 
REAL  WOOT(I) 

REAL  HE 

INTEGER  IENQF 
INTEGER  INITSN 
INTEGER  IFXLE 
INTEGER  OFXLE 
INTEGER  OUTSV1 
INTEGER  PFlLE 

EQUIVALENCE  (OATAU).  DT) 


00000010 

ooooooco 

00000030 

00000040 

00000050 

00000060 

00000070 

OOOOOOSO 

00000090 

00000100 

00000110 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

oooooieo 

00000190 

00000200 

00000210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 

00000260 

00000290 

00000300 

00000310 

00000320 

00000330 

00000340 

00000350 

00000360 

00000370 

00000300 

00000390 

00000400 

00000410 

00000420 

00000430 

00000440 

00000450 

00000460 

00000470 

00000460 

00000490 

00000500 

00000510 

00000520 

00000530 

00000540 

00000580 

00000560 

00000570 

00000560 

00000590 

00000600 


••  •• 


EQUIVALENCE  (DATA(E),  PRNTSW) 
EQUIVALENCE  (DATA!  3),  CUTSWJ 
EQUIVALENCE  (DATA(4)»  XFILE) 
EQUIVALENCE  (0ATAC5).  SPARED 
EQUIVALENCE  <DATA<6)»  SPARES) 
EQUIVALENCE  <DATA(7)»  OELI) 
EQUIVALENCE  C0ATA(8).  QABXt  1 ) ) 
EQUIVALENCE  (DATAC  17),SFP0<  1 ) > 
EQUIVALENCE  <OATA(20),BIAS<1)) 
EQUIVALENCE  (0ATA( 23 ) >QABY( 1 ) ) 
EQUIVALENCE  (DATA( 32 ) »QA3Z( 1 ) ) 
EQUIVALENCE  ( DATA! 41 ) »MODPDT ) 
EQUIVALENCE  (DATA(42),KII(1)) 
EQUIVALENCE  (DATAC45) ,MRC) 
EQUIVALENCE  CDATA(46),QMIS(1)) 
EQUIVALENCE  COATA(55),SFMO(1)) 
EQUIVALENCE  ( DATA! 58 ) » SFP1 ( 1 ) ) 
EQUIVALENCE  (0ATA<61),SFM1(1>> 
EQUIVALENCE  (DATA! 64)  ,RXtl ) ) 
EQUIVALENCE  < DATAC 67), RYil)) 
EQUIVALENCE  (DATA! 70 ) ,RZt D ) 
EQUIVALENCE  (DATAf 73) ,IXX) 


EQUIVALENCE  (PDATA(1),WE) 
EQUIVALENCE  < PDATAC 2 ) ,RE ) 
EQUIVALENCE  (PDATAC  3), G) 
EQUIVALENCE  ( P0ATA< 4) ,PRNT0T) 
EQUIVALENCE  (P0ATAC5),PBUP<D) 


DATA  CROSS3/3#0./ 
DATA  TPRKOQ/'O.O/ 
DATA  TFRNT/O./ 
DATA  INITSW/O/ 
DATA  IFILE/67/ 
DATA  KIO/O/ 

DATA  PFILE/7/ 


IF 

IF 


(IEH0F.EQ.D  RETURN 
(INITSW.EQ.O)  GO  TO  500 


IP  ( T . LT . TCAC- >0001)  RETURN 


C COMPENSATE  FOR  SCALE  FACTOR  ERROR  AND  BIAS 
C 

IP  (K10.EQ.0)  GOTO  25 


20 


22 

C 

25 

C 


00  20  isn.3 

DOTH)  13  )=DTH£TAC  13  l-DTMETPC  13) 
CALL  MXV(QADO*DDTH,WOOT) 

00  28  «n*3 

WOOT(I2)=W)Or(I2)/t)T 

CONTINUE 


CONTINUE 


OO  100  m,s 

IF  (DVOCX).LT.O. ) GOTO  26 
Tl=l , *SPO( 2 ) ASPIC I JRDVOt X J/OT 
GOTO  27 
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00000610 

00000620 

00000630 

00000640 

00000650 

00000660 

00000670 

00000680 

00000690 

00000700 

00000710 

00000720 

.00000730 

00000740 

00000750 

00000760 

00000770 

00000780 

00000790 

00000800 

00000810 

00000820 

00000830 

00000840 

00000850 

00000860 

00000870 

00000880 

00000890 

00000900 

00000910 

00000920 

00000930 

00000940 

00000950 

00000960 

00000970 

00000980 

00000990 

00001000 

00001010 

00001020 

00001030 

00001040 

00G01050 

00001060 

00001070 

0000X060 

00001090 

0Q00110Q 

oooomo 

00001120 

00001130 

00001)40 

00001150 

00001160 

00001170 

oooomo 

00001190 

00001200 


26  Tl«l . +SMO  ( I ) *3M1  ( I )#DVO(  I l/TJT  0000X210 

c 00001220 

27  CONTINUE  00001230 

0V0(  I )*OVO( I )*T1  00001240 

OVO( I )=DVQ( I )+BIAS( I )»DT  . 00001250 

c 00001260 

IF(I.NE.l)  GOTO  30  ® 00001270 

C 00001280 

CROSSK 1 )=DTHETAC  2 )#RX<  3 )-DTHETA( 3)*RX(  2 5 00001290 

CR0SSK2)=0THETA(3)#RX(l)-DTHETAtl)#RX(3)  00001300 

CROSSK  3)=0THETA(  1 )«RX(  2 l-OTHETAI  2 )#RXt  1 ) 00001310 

CR0SS2( 1 )=( OTHETAt  2 )*CR0SS1( 3 )-DTHETA( 3 )#CRDSS1( 2 ) )/OT  00001320 

C 00001330 

CAU  t1XV(QABX>DTHETA,DTHETZ)  00001340 

C 00001350 

IF  (K10.EQ.0)  GOTO  50  00001360 

CROSSK  l)s(0DTH(2  )*RX(3)-D0TH(  3)*RX(2)  )/0T  00001370 

GOTO  50  00001380 

C 00001390 

30  IFCI.NE.2)  GOTO  40  00001400 

CROSSK  1 )=0THETA(  2 )*RTt  3 )-DTHETA(  3 )*RTt  2 ) 00001410 

CROSSK  2 J -DTHETA(  3 )*RY(  1 )-0THETA<  1 J*RYC  3 ) 00001420 

CROSSK  3 )=DTHETA(  1 >*RYt  2 )-DTHETA(  2 )*RYC  1 ) 00001430 

CROSS2C  2 )=(OTHETA(  3 )#CR0SS1(1)-0THETAI  1 )*CR0$3K  3)  )/OT  00001440 


C 00001450 

CAU  MXV(  QABY  »DTHETA»DTHETZ ) 00001460 

C 00001470 

IF  (KlO.EQiO)  GOTO  50  00001480 

CR0SS31 2 ) =( OOTHl  3 )**RY(  1 )-DDTH(  1 >«RY( 3 ) J/OT  00001490 

GOTO  50  00001500 

C 00001510 

40  CAU  MXV(QABZ,OTHETA,OTHETZ)  00001520 

C 00001530 

CROSSK  1 )*0THETA(  2 )#RZ(  3 )-DYMETAt  3)*RZC  2 ) 00001540 

CROSSK  2 )B0THETA(  3 )*RZt  l J-OTHETAI 1 >*RZC  3 1 00001550 

CROSSK  3 l=0THETA<  1 )«RZ<  2 >-DTHETA(  2 )#RZ<  1 > 00001560 

CROSS2(  3 )=(  0THETA(  1 )#CROSSK2  )-DTHETA(  2 )«CR0S31(  1 ) )/OT  00001570 

C 00001580 

IF  (K1Q.EQ.0)  GOTO  50  00001590 

CR0S33t  3 )= ( DOTHt 1 >«RZ<  2 )-OOTH( 2 >WRZ< 1 ) )/OT  00001600 

50  CONTINUE  ' 00001610 

OVO(  I J=OVOt X )+KXX(  I )**DVO(  X )«DVOt  I >/OT-OEU»OTHETZ(  3 )*OTHETZ(  1 )/OT  00001620 
X /MRC  00001630 

OVQI X )=OVO(I)-CRO$S3t X )-CROS32( X 1 00001640 

C 00001650 

IFIK10.EQ.0)  GOTO  100  00001660 

OVOmsDVO(X)»XXX/MRC«UOOT(X>  00001670 

. 100  CONTINUE  000016S0 

KlOal  00001690 

C 00001700 

00  160  16*1*3  00001710 

160  BTHETPt 16 )*0TMETA(  16 ) 00001720 

C 00001730 

C COMPENSATE  FOR  X.T.  ANO  Z HXS5ALIGNMENT  00001740 

C 00001750 

. CAU  KXV(QNISiOVOiOVA)  00001760 

,C  00001770 

C OUTPUT  ANO  PRINT  CONTROL  00001780 

C 00001790 

IF  (MNTOT.GT.O)  GO  TO  960  00001800 


on  o n o o o 


C 


C 


c 


c 


c 


IF  (MODPDT.EQ.O)  GO  TO  999 

00001810 

00001820 

IF  IT.LT.TPRNOD-.0005)  GO  TO  999 

00001830 

TPRN00=TPRM0D+M00PDT 

00001840 

GO  TO  970 

00001850 

00001860 

950  CONTINUE 

00001670 

IF  (T.LT.TPRNT-.0005)  GO  TO  999 

00001880 

TFRNTsTPRNT+PRNTDT 

00001890 

00001900 

970  CONTINUE 

00001910 

IF  (PRNTSW.LT.l. ) GO  TO  999 

00001920 

HRITE(OFILE»1200)  OVA 

00001930 

00001940 

999  CONTINUE 

00001950 

TCAC=T*DT 

00001960 

RETURN 

00001970 

00001980 

ACC  COMPENSATION  MODULE  INITIALIZATION 

00001990 

00002000 

500  CONTINUE 

00002010 

REWIND  IFILE 

00002020 

REWIND  PFILE 

00002030 

501  READ  (IFILE, 1000)  IX.DATA(IX) 

00002040 

IF  (E0F( IFILE))  502,501 

00002050 

502  CONTINUE 

00002060 

503  READ  (PFILE,  1000)  IX.PDATAUX) 

00002070 

IF  (E0F( PFILE))  504,503 

00002000 

504  CONTINUE 

00002090 

00002100 

REWIND  IFILE 

00002110 

REWINO  PFILE 

00002120 

OFILE=XFILE 

00002130 

00002140 

WRITE  ( OFXLE ,10X0 ) OT.PRNTSW.OUTSW, OFXLE, SPARE!, HODPOT,PRNTOT,DELI00002150 

X . BI AS ,KIX ,MRC . IXX , QA8X , GABY , QABZ 

00002160 

WRITE  (OFXLE, 1011)  QM1S.SFMO,SFM1,5FPO,SFP1,RX,RY,RZ 

00002170 

00002180 

00002190 

00  600  12-1,3 

00002200 

BIAS!  12  )"8IASI  12  )»1 . C-6WG 

00002210 

KH(I2)=KXI(I2)**l.E-6/G 

00002220 

600  CONTINUE 

00002230 

00002240 

QABOCl)3QABX(4) 

00002250 

QAQO(  2 )sf}ABX(  5 ) 

OOf 02260 

OABQt  3)=QABX(6) 

00002270 

QADO(4)eQADY<4) 

00002200 

QAEOt  5)kQABY(5) 

00002290 

QABO(6)*QABY(6) 

00002300 

QAUOI  7)aQABZ(4) 

00092310 

QAB0(0)sQA13Z(  S) 

0000235!) 

GADO(9)sQAOZ(6) 

00002330 

IXXsIXX*2.S?E-6 

00002340 

0ELX*0ELI**2.37E-6 

00002350 

HRCaHR(;*7.23E-S 

00002360 

00  650  Xlal.3 

00002370 

SP0( 11 )a$FP0( I1)*1.E“6 

00002380 

SPHIl)sSF?nii)41.E-6/G 

-90002390 

SM0Ul)=SF«0IU)»l.fc-6 

00902400 

1-56 


l>v  •asi*Bfa£± 


SMI  ( II )=SFM1 t II >*1 . E-6/G 
650  CONTINUE 
C 
C 

INITSWsl 

TCAC=T*DT 

RETURN 

C 

1000  FORMAT  (I5.F20.10) 

1010  F0RMAT136H  ACC  COMPENSATION  INITIALIZATION  , 
X/3X.8H  DT  ,3X,Glb.8,3X,4H  SEC, 

X/3X.8H  PRNTSW  ,3X,G16.0, 

X/3X.8H  OUTSW  ,3X,G16.8> 

X/3X.8H  OFILE  >3X.I15i 
X/3X.8H  SPARE  ,3X,G16.8, 

X/3X.8H  MOOPOT  ,3X,G16.8, 

X/3X.8H  PRNTOT  ,3X,G16 .8,//, 

X/3X.8H  DELI  ,3X,G16.6,9H  GM  CM«#2, 

X/18X , "X" , 18X , "Y" , 18X , "Z" 

X/3X.8K  BIAS  ,3t3X,G16.8),8H  MICRO  G , 

X/3X.8H  KII  ,3t3X, 616.8),  13H  MICRO  G/G»»2, 

X/3X.8H  MRC  >3X>G16.8.  6H  GM  CM. 

X/3X.8H  IXX  >3X,G16.8>9H  GM  CM»»2, 

X/3X.8H  QA8X  , 3( 3X, G16.fi ) , 

X/llX  ,3(  3X.G16 .8 ) . /UX , 3(  3X , G16 .8 ) , 

X/3X.8H  QABY  ,31 3X.G16.B), 

X/nX,3(3X,G16.8),/llX,3(3X>G16.8), 

X/3X»8H  QA8Z  ,31 3X,G16 .8) , 

X/llX.  3t  3X.G16 .8 ) ,/llX,  3t  3X.G16 .8 ) ) 

1011  FORMAT  ( 

X/3X.8H  QMIS  ,3<3X,G16.8>, 

X/llX, 3( 3X,G16 .6 ) >/llX , 3(  3X,G16 . 8 ) » 

X/SX.8H  SFMO  ,3< 3X.G16.81,  AH  PPM, 

X/SX.8H  SFM1  ,3(3X,G16.8)>  7H  PPM  / G , 

X/3X.8H  SFPO  , SOX, 016. 8),  AH  PPM, 

X/3X.8H  SFP1  ,3(3X,G16.8),  7H  PPM  /G, 

X/SX.OH  RX  ,30X,G16.8),  3H  FT, 

X/3X.8H  RY  ,3OX,016.8>,  3H  FT, 

X/3X.8H  RZ  , SOX, 616. 8),  3H  FT,/) 

1200  F0RMAT16X.25H  CAC  **  OV  (FT/SEC)  ,3616.8,/) 
END 


00002410 

00002420 

00002430 

00002AAO 

00002A50 

00002A60 

00002A70 

00002A80 

00002A90 

00002500 

00002510 

00002520 

00002530 

000025AO 

00002550 

00002560 

00002570 

00002580 

00002590 

00002600 

00002610 

00002620 

00002630 

00002640 

00002650 

00002660 

00002670 

00002680 

00002690 

00002700 

00002710 

00002720 

00002730 

00002740 

00002750 

00002760 

00002770 

00002780 

00002790 

00002800 

00002810 


o o o o o o 


03/10/76  DATE  OF  CURRENT  MODULE 

THE  GYRO  COMPENSATION  MODULE  COMPENSATES  FOR  ACCELEROMETER  BIASES 
AND  ERRORS. 

SUBROUTINE  GCOMP  (T.XENDF, 

X DTHETO.DVA.OTHETZ) 


REAL  BIAS! 3 ) 
REAL  BUFC17) 
REAL  DATA! 86) 
REAL  OELI 
REAL  DT 
REAL  0THC3) 
REAL  DTHETG13) 
REAL  0THET0J3) 
REAL  0THETZC3) 
REAL  OVA( 3 ) 
REAL  QVG<3) 
REAL  0W0(3) 
REAL  G 
REAL  H 
REAL  IXX 
REAL  KX( 3) 

REAL  KOI  31 
REAL  KSUl 
REAL  KIK3) 
REAL  KXS<3> 
REAL  KOS(  3) 
REAL  KI0(3) 
REAL  KS8(  3) 
REAL  LAT 
REAL  MOOPDT 
REAL  PBUF(16 ) 
REAL  PDATAUO) 
REAL  PRNTOT 
REAL  QGQXtOl 
REAL  QG6Y(9) 
REAL  QGBZ(9) 
REAL  <3MIS(9) 
REAL  0000(9) 
REAL  RE 
PEAL  SFMO( 3) 
REAL  SFMU3) 
REAL  SFP0(3) 
REAL  SttOtS) 
REAL  SMlt 3) 
REAL  SPQtS) 
REAL  SPM3) 
REAL  SPARE1 
REAL  SPARES 
REAL  3FPK3) 
REAL  T 
REAL  TI 
REAL  TPRMOO 
REAL  TPRNT 
REAL  WOOT( 3) 
REAL  HE 


1-58 


XSKSssseaaaam 


00000010 

00000020 

00000030 

00000040 

00000050 

00000060 

00600070 

OOOOOOSO 

00000090 

00000100 

00000110 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

00000160 

00000190 

00000200 

00000210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 

00000260 

00000290 

00000300 

00000310 

00000320 

00000330 

00QQQ340 

00000350 

00000360 

00000370 

00000380 

00000390 

09000400 

00000410 

00000420 

00000430 

00000440 

00000450 

00000460 

00000470 

00000460 

00000490 

00000500 

00000510 

00000520 

00000530 

00000540 

00000550 

00000560 

06000570 

00000560 

00000590 

00000600 


o o ft  o n 


i 


C 


c 


c 


.e 


INTEGER  IEN0F 
INTEGER  IFILE 
INTEGER  2NITSW 
INTEGER  OUTSW 
INTEGER  OFILE 
INTEGER  PFILE 


EQUIVALENCE  < DATAC 2 »,  PRNTSW) 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 


(DATA(3).  OUTSW) 
(DATA<4),  XFILE ) 
(OATA(S),  SPARED 
(DATA(6 )«  SPARE2) 
(OATAt  7),  QMIS(D) 
(DATA<1?),SFP0(D> 
t OATAt  20), BIAS!  D) 
( DATA!  23),SFM0t  D ) 
(0ATAf26),QGSXfD) 
tDATAI  35) »QGBY(  1 ) ) 
(OATAt  44  JiQGBZtU) 
t OATAt  53 ) >KO(  1 ) ) 

( OATAt 56 ) iKIt  D ) 
(OATAt  59  ))KS(  1 ) ) 
tDATA(62),KII(D) 
(DATA(65),H) 

( DATA! 66 ) ,IXX) 

1 0ATA(  67 ) iKS3(  1 ) ) 
(OATAt  70  ),KIO.‘D) 

< OATAt  73  ),KCS<D) 

( OATAt  76  ),KIS(D) 
(DATA!  79), DELI) 
(DATA(aO).SFtH(D) 
IDATA(63)iSFPI(D) 

( OATAt 06 ) (MOOPOT ) 


EQUIVALENCE  (PQATAm.WE) 
EQUIVALENCE  IP0ATAC2 ),RE » 
EQUIVALENCE  (PDATAt  3)>G) 
EQUIVALENCE  t PDATAt  4 ),PRNTDT> 
EQUIVALENCE  <PQATAt5>,paUFtl>) 


DATA  TPRN0070 . 0/ 
DATA  TPRNT/O.O/ 
DATA  XFILE/W 
DATA  INITSWXO/ 
OATA  Kl  / 0 / 

DATA  PFILE  /?/ 


if  tIENOP.EQ.D  RETURN 
IP  tlNITSN.EQ.O)  GO  10  500 
If  tt.LT.TCGY-. 00051  RETURN 


00  100  m.3 

COMPENSATE  FOR  SCALE  FACTOR  ERROR  AW  BIAS 

IF  tOTHETOm.LT.O.)  GOTO  90 
C POSITIVE  SCALE  FACTOR  ERROR 


00000610 

00000620 

00000630 

00000640 

00000650 

00000660 

00000670 

00600660 

00000690 

00000700 

00000710 

00000720 

00000730 

00000740 

00000750 

00000760 

00000770 

00000760 

00000790 

00000600 

00000610 

00000620 

00000630 

00000640 

00C00550 

00000660 

00000670 

00000660 

00000690 

00000900 

00000910 

00000920 

00000930 

00000940 

00000950 

00000960 

00000970 

00000960 

00000990 

00001000 

00001010 

00001020 

00001030 

00001040 

00001050 

00001060 

00001070 

00001060 

00001090 

00001100 

oooomo 

00001120 

oooomo 

00001140 

00001150 

00001160 

00001170 

00001160 

OOOOUvO 

00001200 


1-S9 


n n try  n ooon  r.  n a o o ononooo  oon 


C 000012X0 

Tl=l . *SP0t  I HSPK I )»DTHET0f  I )/0T  00001220 

GOTO  95  00001230 

00001240 

NEGATIVE  SCALE  FACTOR  ERROR  00001250 

00001260 

10  Tl=l.*SM0m»SMl(I)«DTHET0m/DT  00001270 

15  OTHETOm=DTHETOm*Tl+3IASil>*DT  - 00001260 

00001290 

.00  CONTINUE  00001300 

00001310 

00  300  11*1,3  00001320 

00001330 

TRANSFORM  ACCELERATION  FROM  XYZ  TO  I0S  00001340 


00001350 

00001360 

00001370 

00001360 

00001390 

00001400 

00001410 

00001420 

00001430 

00001440 

00001450 

00001460 

0000147C 

00001460 

00001490 

00001500 

00001510 

00001520 

00001030 

00001540 

00001550 


COMPENSATE  FOR  ACCELERATION  EFFECTS  00001560 

00001570 

OTHETOt  II  )*OTHETO(  11  UKIIII  >»0V6t  1 WK0(  II  >«DVG(  2 UKSI XI  >«  00001500 

X 0VG(  3 1*< KOS(  II  J*DVG(  2 >*9VG<  3 >*KS3l  II  >*DVG(  3 >*DVG<  3 H 00001S90 

X Kill  XI  >*DVG(  1 )“0VGI  1 )«KISt  II  )*DVG(  1 )*DVGt  3 )+KI0( XI IPOVGt  1)«  00001600 

X 0Vfl( 2 ) 1/0 Y-DELI*OYHETGt 1 )*0TH6TGl 3 1/H/DT  00f01610 

00001620 

>00  CONTINUE  00001630 

ir  <Kl.fQ.01  GOTO  410  00001640 

DO  950  I5al,3  00001650 

00001660 

CALCULATE  HOOT  00001670 

00001660 

LWU5l*OYH6YOt:S>«OtNUS>  00001690 

>50  CONTINUE  00001700 

call  MXVfqoaa.xDOY.own  oooomo 

C 00001720 

0C  460  I2*1.3  00001730 

otn(T:'«cthfto(I2I  00001740 

OTHETOI  12  )*OTHCY0l  12  1*IXX»0N0(  12  l/H/OT  00001750 

400  CONTINUE  00001760 

ZC  00001770 

COTO  460  00004760 

410  Kl*l  00001790 

00  450  14*1,3  00001600 


I 


IF  (Il.NE.l)  GOTO  210 
X GYRO 

CALL  MXV1 QGQXiOVAiOVG) 

CALL  MXVtQGBX.DTHETO.OTHETG) 
SOTO  230 

210  IF  (I1.NE.2)  GOTO  220 
Y GYRO 

CALL  MXVIOGBY • OVA > DVR ) 

CALL  NXV( CG5T , OTHE  TO tDTHETG ) 
GO  TO  230 
2 GYRO 

220  CALL  MXV( QGBZ , OVA , QVG 1 

CALL  MXV( QGBZ « DTHETO , OTHETG ) 


ira  rnitvtDiiP 


oono  on  no  o o non  ooo 


OTHC 14 )=0THET0( 14 ) 

450  CONTINUE 
460  CONTINUE 

COMPENSATE  FOP  X.Y,  AND  Z MISALIGNMENT 

CALL  HXVT  QMS  tOTHETO  > DTHETZ ) 

PAINT  OUTPUT  AND  CONTROL 

IF  ( PfiNTDT .GT.O ) GO  TO  960 
IF  (MOOPOT.EQ.O)  GO  TO  999 

IF  ( T. LT*  TPRHOD* .0005)  GO  TO  999 

TPRMQO=TPRMOO*MOOPOT 

GO  TO  970 

960  CONTINUE 

IF  (T.IT.TPRNT-.000S)  GO  TO  999 
TPRNTsTPSNT+PRNTUT 

970  CONTINUE 

IP  CPRNTSW.LT.l.)  GO  TO  999 
WRITE! OFX  LE <12001  DTHETZ 


999  CONTINUE 
TC5Y=T«0T 
RETURN 


INITIALIZATION 

500  CONTINUE 
REMIND  PFILC 
REMITS)  XFILE 

501  BEAD  (IFILE.1000)  XX.OATAtlX) 

IF  <60F»IFItEn  $02, $61 

SOI  CONTINUE 

503  READ  (PFUE, 7.0001  I?,PeAfAUX> 

IF  lEOFWXLEll  510,505 
5X0  CONTINUE 

REMIND  Pf IlE 
REMIND  SPILE 
UFUeeXPlte 

,c 

WRITE  TOPItS.lOlO)  OT,FRNT5M,OUT5H,OmE,HOOWt,PfiNTOT. 

X 0lAS,5FP0,SPN0,«Ce>!,t}5OT,«SlJ2.0NTS 
WfE  'tOF lit *5011 1 H1,KO,P,S,k;I»KI5,KOS.N$3,HSO,HiOEU.SXX 
WRITE  !OFSLC»  10121  5FW,3?P1 
C 

oo  $se  nn.i 

sums  ^SFMt  It  )•!.£-* 

SPIl  illa5P?lMU»l.E*6 
SP0llX|iSFP0<n>«H.E-6 
S.'lom»«'SFMO»SU*l.E-6 
550  CONTINUE 
C 


OOOOIfllO 

00001020 

00001630 

00001040 

00001050 

00001660 

00001070 

ooooieoo 

00001890 

00001900 

00001910 

00001920 

00001930 

00001940 

00001950 

00001960 

00001970 

00001900 

00001990 

00002000 

00002010 

00002020 

00002030 

00002040 

00002050 

00002060 

00002070 

00002000 

00002090 

00002100 

00003110 

00002120 

00002130 

00002140 

000031S6 

00002160 

00002170 

eoocaeo 

00002190 
00002290 
0G03221O 
00002220 
00008230 
09002240 
00008850 
00002860 
00002270 
00 00? 280 
08002890 
09008300 
090013*0 
00002380 
09008330 
00092340 . 
00008350 
00002350 
00008370 
00002336 
00002390 
00902400 


524 

C 


DO  524  13*1.3 

&US(  IS  )*BIAS(  13  )*4.05E-6 

tutxs)«Kxtx3!#4.ose*6/8 
KO(X3)*KOtX3)*4.eSE"6/G 
KStX3)*KS(ism.a3e-^o 
KXI!  13 )»KXt(  131*4.981*6/0/8 
KI51I3)*KX3!  XS)*4.65E-6/8/0 
KOSl  IS)*KOSt  I3)*4,65E~6/G/0 
K83!  X3)*KSS! X3W.85E-6/6/G 
KXO(  XS)*KXQt  I3)*4.95l“6/G/G 
CONTINUE 

IXX»XXX*2.37E*6 
0ELI«0EU*2.3?E-6 
N*H*2 . S7E-6 
«0l)GtlI*QGBXt4) 
QCBGt2>at}3l'X!8) 

QCSG! S)aQGBX!6) 


ion 


00008410 

00002420 

00008433 

00008440 

00002480 

00002460 

00002470 

00002430 

00008440 

00002800 

00002810 

00002820 

00002830 

00002840 

00002550 

00008560 

00002570 

00008500 


Q0SC!t4)*Q0BV(4) 

00008S40 

G0B3!51*GG:JYt8) 

00002600 

<8000!  6)  *0007(6) 

00002610 

<3006!  71*15002!  4) 

00008620 

£5CD6(0)*UG8I!5) 

00002630 

Q080(9»«8GDZ(6) 

00008640 

00002680 

00002660 

INITSW*! 

0900C670 

TC6Y*T*DT 

0000C6S0 

RETURN 

00002640 

00002700 

00002710 

FORMAT  (X5.F20.1O) 

00002720 

FORMAT! 36M  OYR  COMPENSATION  INXTXAL12ATX0N  , 

00002730 

X/JX.0M  DT  ,SX,G16.0,3X,4H  32C. 

00002740 

X/SX.OH  PROTON  ,SX,Q16.6, 

00002780 

X/SX.9M  OUT8N  ,3X, 016.0. 

00008760 

X/SX.6H  OFXIE  .3X.US, 

00002770 

X/SX.6H  HOt) Ft) T .3X.816.6. 

00002730 

x/sx.en  t'k'NTor  ,3x,oi6,e.// 

. 

000027‘K' 

X/16X » "X” 1 10X , "Y" , 13X . "2" 

00003000 

X/SX.6M  MAG  ,3(  JX.CU6.6) 

,7H  DEO/HR, 

00008910 

X/3X.6H  »FP0  ,3t IX, 816,0) 

, 4M  PPM, 

ooooraeo 

X/SX.8H  OF M0  .3t  3X.G16.3) 

, 4N  PPM,/, 

oooonosa 

X/SX.OH  OOOX  .31 3X.G16.0) 

, 

00002340 

X/UX,lt  3N,Gi6.0)»/llX,StSX 

,016.0),/, 

00002650 

X/SX.OH  GOBY  ,3t3X, 816.0) 

, 

00008060 

X/UX,3(  3X.016.0)  ,/llX,3t  3X 

,016.01,/. 

00008670 

X/3X.6H  0602  ,31 3X, 016.0) 

t 

00008000 

X/11X.  Jt  SX.G16.0)  ,/HX»3(  3X 

.016.01. 

00002640 

X/SX.OH  QM1S  ,3(SX, 816.0) 

, 

00002400 

X/nX,3t3X,016.0),/nX,3(  JX 

,016.0),/) 

00008410 

FORMAT! 

00002420 

X/SX.OH  HI  ,3! 3X, 016,0) 

,4H  OEO/HR/O. 

00002430 

X/SX.OH  KO  ,3! 3X, 016.0) 

,4H  DEO/IIR/O. 

00002440 

X/SX.8H  K8  ,3! SX.016,6) 

,4(1  OEO/NH/Ot 

00008450 

X/SX.OH  klX  ,3«3X, 016,0) 

,161!  DEG/HR/Q**e. 

00008460 

X/SX.OH  K18  ,3! 3X, 016.0) 

, 16H  DE0/HR/G**2. 

00002473 

N/3X.OH  KO!)  , 3!  3X. 016,0 1 

,16H  »EQ/MR/G**C. 

60002490 

X/3X.0H  KStl  .3!  JX, 016.0) 

,16H  DEG/MR/0**C, 

00008440 

X/SX.OH  KXO  ,3! SX» 816.0) 

,1611  0E0/NR/G**C, 

00003000 

1-62 


I 


X/3X.6H  H >3X*G16.0(13H  GM  CM*«2/SEC,  00003010 

X/3X.0H  DELI  ,3X. 616.4. 9H  CM  CM*«2,  00003020 

X/3X.0H  XXX  ,3X,ei6.0,9H  Qrt  CMim*2J  00003030 

1012  FORMAT ( 00003090 

X/SX»8H  SFMl  >3(3XtG16.8)il3H  PPM  /RAD/SEC,  00003050 

X/3X.8H  SFP1  .3(3X, 816.61,138  PPM  /RAO/SEC,//)  00003060 

1200  F0RMAT16X,27H  #*  CGY  DTHETA  (RADi  .3G16.0,/)  00003070 

END  00003060 


£ 


| 

i 


« 


* 


I 


I 


C 3/15/73  DATE  OF  CURRENT  MODULE 
C 

C 10/18/77  CHANGE  FORMATS  TO  G16.8 
C 

#*»##  RING  USER  GYRO  COMPENSATION  MODULE 
C 
C 

SUDROUTINE  GCOMP  ( T.IENOF.DTHETO.DVA, 

, OTHETZ  1 


*«««« 


C 

C .DTHETO  - INCREMENTAL  ROTATION  GENERATED  BY  GYROS. 

C .DTHETZ  - COMPENSATED  INCREMENTAL  ROTATION. 

C .OVA  - NOT  USED. 

C 

IMPLICIT  REAL  ( A-Z  ) 

INTEGER  I,H.N0F,INITSW,IFILE,IX,J,K,OFILE,NP,PFILE 
DIMENSION  AB9<  3 ) , ANGt  3 ) , ANGRWt  3 > .ANGWNl  3)  , DATAt  31 ) .OTHETZt  3) , 

. 0THETQ<3),0B<3l,DTA<3),DTCt3)»EXP0(3>.EXPSF(3),KWm.SF<3>. 

. SFTAt 3 ) ,SFTC( 3 ) »VTl(3)»DVAt 3 ) . PBUF ( 16 > ,PDATA(  20 ) 

C 

C 

EQUIVALENCE  (OATAtD,  DT) 

EQUIVALENCE  (DATAt  2 ).  PRNTS111 
EQUIVALENCE  (DATAt 3) * OUTSM) 

EQUIVALENCE  IQATA(4),  XF1LE ) 

EQUIVALENCE  ( DATAt 5. ‘ • SPARED 
EQUIVALENCE  ( DATAt  6 ) » SPARED 
EQUIVALENCE  (DATAt 7).  MODPDT ) 

EQUIVALENCE  (DATAt Si.  DDtl)i 
EQUIVALENCE  (DATAt ID,  DTAtli) 

EQUIVALENCE  ( DATA! 14 i,  DTCtDl 
EQUIVALENCE  (0ATA<17),  SFTAt D 1 
EQUIVALENCE  (DATAt 20 >,  SFTCtD) 

EQUIVALENCE  (DATAt 23),  KWID) 

C 

C 

EQUIVALENCE  (PDATA(DiWE) 

EQUIVALENCE  (POATA(2l,RE) 

EQUIVALENCE  ( POATAt  3) ,G1 
EQUIVALENCE  t POATAt  <* > » PRNTOT ) 

EQUIVALENCE  tP0ATA(5l,P8UFUJ) 

C 

DATA  DTR  /. 01745329251994330/ 

DATA  XNITSW  /0/ 

DATA  XFILE  /69/ 

DATA  OF  HE  /6/ 

* DATA  PFILE  /7/ 

DATA  TPRNT  /0.0/ 

DATA  TPRNOO  /O.O/ 

C 

IF  tXENDF.EQ.l)  RETURN 
IP  tINITSU.EQ.O)  GO  TO  500 
IP  (T.LT.TCGY-. 00911  RETURN 
C 

-C -BEGIN  NORMAL  COMPUTATIONS * — * 

C»»TRAN9IENT  DRIFT  ANO  TRANSIENT  SCALE  FACTOR*  OTA.SFTA. 

100  DO  105  I * 1,3 

IF  ( AB3(  DTAtD  ) ,IT.  l.E-10  i DTAtl)  a 0. 


00000010 

00000020 

00000030 

00000040 

00000050 

00000060 

00000070 

00000060 

00000090 

00000100 

00000110 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

00000180 

00000190 

00000200 

00000210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 

00000280 

00000290 

00000300 

00000310 

00000320 

00000330 

00000340 

OOOOQ35Q 

00000360 

00000370 

00000380 

00000390 

00000400 

00000410 

00000420 

00000430 

00000440 

00000450 

00000460 

00000470 

00000460 

00000490 

00000500 

00000510 

00000520 

00000530 

00600540 

00000550 

00000560 

•00000570 

00000500 

09000590 

00000600 
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I 


IF  ( ABS(  SFTAtl)  ) .LT.  l.E-10  ) SFTAd)  = 0. 
DTAd)  a OTAt  I )»EXPD(  I ) 

SFTAd)  = SFTA(  I )#SXPSF(  I ) 

K=4*d-1)+1 

KW(K)  = SF(I)  ♦ SFTAd) 

105  CONTINUE 

C*«C0MPUTE  COMPENSATED  INCREMENTAL  ROTATION:  DTHETZ. 
00  110  I = 1,3 

110  VTld ) = OTHETOd)  - ( D3(I)  + DTAd)  )«DT 
CALL  MXV  ( KW,VT1 (OTHET2  ) 

DO  112  I = 1,3 

112  OTHETZd)  = VTld)  - DTNETZd) 

c end  normal  computations 

C 

C OUTPUT  AND  PRINT  CONTROL 
C 


00000610 

00000620 

00000630 

00000640 

00000650 

00000660 

00000670 

00000680 

00000690 

00000700 

00000710 

00000720 

00000730 

00000740 

00000750 

00000760 


IF  (PRNTCT.ST.O)  GO  TO  960 
IF  (MOOPDT.EQ.O)  GO  TO  999 
C 

IF  ( T.LT .TPRMOD- .0005)  GO  TO  999 
TPRMD0=TFRMC0+HQ0PDT 
GO  TO  970 
C 

960  CONTINUE 

IF  (T.LT.TPRNT-,0005)  GO  TO  999 
TPRNT  aTPRNT+PRNTOT 
C 

970  CONTINUE 

IF  (PRNTSU.LT.l)  GO  TO  999 


C 

WRITE (OFILE ,1200 ) DTHETZ 
C 
C 

999  CONTINUE 
TCGY=T4DT 
RETURN 


C 

C- 


-INITIALIZATION  SECTION- 


C*M»READ  THE  INPUT  OATA  PILE*  XFXLE. 


C 

C 

C 

C 

C 

C 

c 

c 

„c 


.DT  - SIMULATION  TIME  STEP 

.OB  - BIAS  DRIFT 

.OTA  - DRIFT  TRANSIENT  AMPLITUDE 

.OTC  - DRIFT  TRANSIENT  TIME  CONSTANT 

.SPTA  - SCALE  FACTOR  TRANSIENT  AMPLITUDE 

•SFTC  - SCALE  FACTOR  TRANSIENT  TIME  CONSTANT 

.KU  • SF  AND  GYRO  XA  MISALIGNMENT  MATRIX 


500  CONTINUE 
REWIND  PFILE 
REWIND  XFXLE 

501  READ  (XFXLE, 1000)  XX.DATAIXX) 
IF  (SOFdPILE))  SOS, SOX 

502  CONTINUE 

503  READ  I PFILE,  1000)  XX.POATAdX) 
. IF  tlOP(PFXLEI)  5X0,503 
.5X0  CONTINUE 

REWIND  PFILE 
REWIND  XFXLE 
OFXLE=Xme 


00000770 

00000780 

00000790 

OOOOOOOO 

00000810 

00000820 

00000830 

00000840 

00000850 

00000860 

00000870 

00000080 

00000890 

00000900 

00000910 

00000920 

00000930 

00000940 

00000950 

00000960 

00000970 

... 00000900 

00000990 
( SEC  ) 00001000 

(DEG/HR)  00001010 
(DEG/HR)  00001020 
( MIN  1 00001030 

t PPM  ) 00001040 
( MXN  ) 00001050 
( PPM  ) 00001060 
00001070 
OOOOIOQQ 
00001090 
OOOOUCO 
00001110 
00001120 
00001130 
00001140 
00001150 
00001160 
00001170 
00001100 
00001190 
00001200 
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WRITE  (OFILE.IOIO)  DT,PRNTSW,OUTSW,OFXLE,PRNTDT,NODPOT 
WHITE  (OFILE.IOII)  DB » D TA  > DTC » SFTA , SFTC > KW 


C 

C**SCALE  THE  INPUT  DATA  TO  INTERNAL  PROGRAM  UNITS. 
A 00  10  I = 1,3 

03(1)  » DB(Il#QTR/3600. 

OTA(I)  = DTA( I >#DTR/3600. 

OTC(I)  = DTCfI)#50. 

SFTA(I)  a SFTAUM.E-06 


SFTC(I)  s SFTC(I)«60. 

OTHETZ(I)  = 0. 

00  10  J = 1,3 

K=3»(I-1)+J 

KW(K)  a KWlKS#i.E-06 

10  CONTINUE 
C 

00  11  X a 1,3 
K=4»(I-1W1 
SF(  I)  = KW(K) 

EXPO(I)  s 0. 

EXPSF(I)  = 0. 

IF  ( DTC(I)  ,GT.  0.)  EXPO(I)  = EXPI-DT/OTCd)  ) 
IF  ( SFTC(I)  .GT.  0.)  EXPSF(I)  = EXP( -OT/SFTC< I ) ) 

11  CONTINUE 
C 

INITSWsl 

TCGY=T*QT 


RETURN 

C*t»END  INITIALIZATION. 

C 

C 


1000  FORNAT  (X5,F20.10) 

1010  F0R«AT(A2H  LASER  GYRO  COMPENSATION  INITIALIZATION  . , 
X/3X»8H  DT  ,3X,G16,A.3X.4M  SIC, 

X/3X.6H  FRNT°"  ,SX  '••16.8, 
x/sx.eH  out;  a ,3X,cw.e, 

X/3X.6H  OFXLE  ,3X,I1S, 

X/3X.6H  PRNTOT  ,3X,G16.8,3X,4H  SEC, 

X/3X,F''  i.jOPOT  ,3X,G16.6,3X,4H  SEC.//) 

1011  Ft)  >,AT( 

X/l  0>‘ , "X" , 16X » **Y" , 18X » "Z" , 


X/3X.AH  08 
X/3X.8H  OTA 
XVSX.AH  DTC 
X/3X.3H  SFTA 
y *3X>8H  SFTC 
X.'3X,8H  NWs 
X/11X*3(  3X,G16 
1200  F0StlAT(6X,27H 
END 


,3(3X,G16.6),7H  DEO/NR* 
,3( JX.G16.8)»7H  DEG/HR, 
,SI3X,G16.«>,4H  MIN, 
,3(3X.G16.8),4H  PPM, 
,3(3X,G16.AMH  HIM, 
,3<3X.G16.8),4H  PPM, 
>6),/XlX,3t3X«G16.A),/V} 
M«  CGY  M»  DTHETA  IRAQ) 


,3(  JX.G20.12)) 


00001210 

00001220 

0000^230 

00001240 

00001250 

00001260 

00001270 

00001260 

00001290 

00001300 

00001310 

00001320 

00001330 

00001340 

00001350 

00001360 

00001370 

00001300 

00001390 

00001400 

00001410 

00001420 

00001430 

00001440 

00001450 

00001460 

00001470 

00001480 

00001490 

00001500 

00001510 

00001520 

00001330 

00001540 

00001550 

00001560 

00001570 

00001580 

00001590 

00001600 

00001610 

00001620 

00001630 

00001640 

00001650 

00001660 

00001670 

00001680 

00001690 

00001700 

00001710 
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o o n o c o o 


00000010 

05/30/73  DATE  OF  CURRENT  MODULE 

C000G320 

00000030 

THE  ATTITUDE  AND  ALGORITHM  MODULE  COMPUTE?  THE  %ODY  TO  INERTIAL 

00600Q90 

TRANSFORMATION.  THE  DIRECTION  COSINE  MATRIX.  ANO  TRANSFORMS  THE 

00000050 

INCREMENTAL  VELOCITY  COORDINATES. 

00000000 

00000070 

SUBROUTINE  ALG  ( T . IENDF .DTHETI »OV , 

00000030 

X 

OVN.DCM) 

0000009C 

REAL 

Cl 

00000100 

REAL 

C2 

00000110 

REAL 

C3 

00000120 

REAL 

COSLAT 

00000130 

REAL 

COS LON 

00000190 

REAL 

COSP 

00000150 

REAL 

CCSR 

00000160 

REAL 

COSH 

00000170 

REAL 

COSY 

00000180 

REAL 

D 

00000190 

REAL 

0ATAU9) 

00000209 

REAL 

DT 

00000210 

REAL 

DTHETA(3) 

00000220 

REAL 

DTHETK3) 

00000230 

REAL 

DTHP?  3 ) 

00000290 

REAL 

DTHPRE(S) 

00000250 

REAL 

DTNRM 

00000260 

REAL 

DV(3) 

00000270 

REAL 

DVH3) 

00000280 

REAL 

0VNI3) 

00000290 

REAL 

01 

00000300 

REAL 

G 

00090310 

REAL 

ILAT 

00000320 

REAL 

I LON 

00000330 

REAL 

LATERR 

00000390 

REAL 

10ELT2(9,9I 

00000350 

REAL 

LOTH? 31 

00000360 

REAL 

LONERR 

00000370 

REAL 

MOOPDT 

00000380 

REAL  ORDER 

00000390 

REAL 

QIO( 9 > 

00000900 

REAL 

QIPI9) 

00000910 

REAL 

QPS19) 

00000920 

REAL 

QUAY?  9) 

00000930 

REAL 

QTEHP?9> 

00000990 

REAL 

POATAI20) 

00000950 

REAL 

PITCH 

00000960 

REAL 

PXTERR 

00000970 

REAL 

PRNTOT 

00000980 

REAL 

RDTOOO 

00000990 

REAL 

RE 

09000500 

REAL 

ROLL 

00000510 

REAL 

ROIERR 

00000520 

REAL 

SINLAT 

00000530 

REAL 

SINLON 

00000590 

REAL 

SINP 

00000550 

REAL 

MNR 

00000560 

REAL 

SINN 

00000570 

REAL 

81  MY 

00000580 

REAL 

QOROCM 

00000590 

REAL 

T 

00000600 

REAL  TALG 
REAL  TPRKCQ 
REAL  TPRHT 
REAL  UDELT(9*9) 

REAL  UDTH2 
REAL  ME 
REAL  HANDER 
REAL  HANERR 
REAL  YAM 
REAL  YAMERR 

REAL  DTH19) 

REAL  ATEMt 9) 

REAL  DCt1(9) 

REAL  Q1S,Q2S,Q3S 
REAL  Q01.Q02.Q03 
REAL  G12.Q23.Q31 
REAL  TFMt 9) 

REAL  DTEIK9) 

REAL  SMET 
REAL  CMET 
C 

INTEGER  NQDCM 
INTEGER  IENOF 
INTEGER  INITGH 
INTEGER  IFILE 
INTEGER  OFILE 
INTEGER  OUTSH 
INTEGER  PFILE 

r» 

EQUIVALENCE  (OATAIX)i  Ot) 
EQUIVALENCE  <OATA<2),  PRMTSW) 
EQUIVALENCE  <0ATA<3),  CUTSU1 
EQUIVALENCE  <DATA<9),  XFXLE ) 
EQUIVALENCE  (OATAtS)i  QOROCN) 
EQUIVALENCE  (0ATA16),  HODPOT) 
EQUIVALENCE  tOATAt II ) .ORDER  ) 
EQUIVALENCE  ( OAYA( 1 2 ) ,OTNRM ) 
EQUIVALENCE  (OATAt 13>*OTSlOH) 
EQUIVALENCE  <0ATAtl9),LATEKR> 
EQUIVALENCE  (DATAU5)  tLONERR ) 
EQUIVALENCE  tOATAt  16  >, MANERR  I 
EQUIVALENCE  (OATAtlThPITERR) 
EQUIVALENCE  tOATAt 13  )*ROLERft) 
EQUIVALENCE  (0ATAU9>,YAU2Ra> 

C 

EQUIVALENCE  tPOATAtlbUE) 
EQUIVALENCE  tPOATAtgJ.RE) 
EQUIVALENCE  (POAYAtSI.G  ) 
EQUIVALENCE  <P3Ant9),PSNTDT) 
EQUIVALENCE  (POATAtS).  HAY) 
EQUIVALENCE  tPDATA<6>.  XLQNI 
EQUIVALENCE  tPQATAt  7)*  HANDER  I 
EQUIVALENCE  <PDATA<9),  ROLL) 
EQUIVALENCE  tPPATAUO),  PITCH) 
EQUIVALENCE  IPOATAt II )«  YAM) 

C 

OAT  A ROTQOG  757.29577951/ 

DATA  TPRH2D/0 , 0/ 

DATA  TPRNT/O.O/ 


00000610 

00000620 

00000630 

00000690 

00000650 

0000066Q 

00000670 

00000600 

00000690 

00000700 

00000710 

00000720 

00000730 

00000790 

00000750 

00000760 

00000770 

00000760 

00000790 

oooooaoo 
00000610 
00000620 
00000630 
00000090 
00000330 
00000360 
00000370 
00000300 
00000090 
00000900 
0Q000910 
00000920 
00000930 
00000990 
00000950 
00000960 
(SOI  00970 
00000900 
08000990 
OOOOIOOO 
00001019 
00001020 
00001030 
00001090 
00001050 
00001060 
00001070 
OOOOIOOO 
00001090 

oooouoo 

oooomo 

00001120 

00001130 

00001190 

00001150 

00091160 

00001170 

00001130 

00001190 

00001200 
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DATA  INITSM/O/ 

00001210 

DATA  IFILE/70/ 

00001220 

DATA  Kl/O/ 

00001230 

OATA  PFILE  n/ 

00001240 

C 

00001250 

c 

00001260 

IF  (IENOF.EQ.1)  RETURN 

00001270 

IF  t INITSW.EQ.Q ) 60  TO  500 

00001280 

IF  (T.LT.TALG-.OOll  RETURN 

00001290 

C 

00001300 

C STORE  OCN  AS  DCMQLO  FOR  USE  ZN  INTERPOLATED  OCH.  QCMHIO 

00001310 

C 

00001320 

00  no  12=1,9 

00001330 

DCMOLDt 12  )=QIBt IS ) 

00001340 

110  CONTINUE 

00001350 

C 

00001360 

00  111  13*1.3 

00001370 

PTNETAt 13  )=DTHETIt 13 ) > 

00001380 

DT.HPt  13  >=OTHETI(  13) 

00001390 

111  CONTINUE 

00001400 

00001410 

: CHOOSE  QUATERNION  OR  DIRECTION  COSINE  MATRIX  UPDATE  CALCULATION 

00001420 

j 

00001430 

IF  tNQQCJI  .EQ.  1)  00  TO  600 

00001440 

2 

00001450 

I BEGIN  QUATERNION  CALCULATION 

00QD1460 

00001470 

UOELTt  1.2  )=-DTHETAt  1 ) 

00001480 

UDELT(X.3)=-DTHETAt2T 

00001490 

U0ELTtl,4)a-QTHETAt  3) 

00Q01500 

UOELTt  2 . 1 J=DTHETAt 1 1 

00001510 

UOELTt 2.3 lBOTHETAI  31 

00001520 

UOELTt  2 .4  l=«DTHETAt 2 ) 

00001530 

UOELTt  3.1  )=OYHETA( 2 ) 

00001540 

UOELTt  3,£ Is-PYHETAt  3i 

00001550 

UOELTt  3 .4 )«OTHETAt 1 1 

00001560 

UDELY<4.l)*DYHETA<3> 

00001570 

UOELTt 4.2 1=07HETAI 21 

00001500 

UOELTt  <*.3t*-0THETAtl> 

00001590 

IF  (NORDER.NE.l)  GOTO  140 

00001600 

C 

00001610 

CO  120  12*1,4 

00001620 

C FIRST  ORDER  QUATERNION 

00001630 

QTEHPt  IS  )«QUAT(  12  >*2 . 

00001640 

C 

00001650 

00  ISO  JSB1,4 

00001660 

If  tX2.eq.J5i  GOTO  120 

00001670 

QTEWtM  latJCELtt  12,-12  )*DUAYl  J2  X.QTEHPC  It ) 

0000X690 

120  CONTINUE 

00001690 

C 

00001700 

00  130  13*1.4 

00001710 

•5UAT(  I J )=QTENP<  131/2. 

00001720 

130  CONTINUE 

00001730 

C 

C0001740 

GOTO  200 

60001750 

140  CONTINUE 

00001760 

UDTHg*-tOTHEtA( J 1*GTHETAU  )*Dt HETAt 2 )*OTWETAt t 1*0  YHETAI 5)» 

00001770 

X DtHETAOtl 

00001700 

C 

00001790 

IF  (NOROER.NE-2)  GOTO  179 

00001900 
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Kl=l 

00001810 

C SECOND  ORDER  QUATERNION 

00001820 

Cl=2.+U0TH2/4. 

00001830 

C 

00001840 

DO  150  14=1,4 

00001850 

QTENP<  14 ) -QUAT  ( 14 ) #C1 

00001860 

c 

00001870 

00  ISO  J4=l,4 

00001880 

IF  ( J4.EQ.I4)  SOTO  150 

00001890 

QTEMPJ  14 >=UDELT< 14 , J4 )*QUA7<  J4 ) ♦QTENP( 14 ) 

00001900 

ISO 

CONTINUE 

00001910 

c 

00001920 

DO  160  15=1,4 

00001930 

CliAT  ( 15 ) =QTEMP(  15  i/2 . 

00C01940 

160 

CONTINUE 

00001950 

w 

00001960 

GOTO  200 

00001970 

170 

IFIN0R0ER.NE.3)  GOTO  300 

00001980 

C 

00001990 

C THIRD  ORDER  QUATERNION 

00002000 

IF  (K1.EQ.0)  GOTO  145 

00002010 

LDTHt  1 )=DTHPR£!  2 )*OTHETA<  3 l-DTHFRE 1 3 )#DTHETAt  2 ) 

00002020 

LDTHI  2 )=DTHPRE(  3i*0T)tETA(  1 1-OTHPSEI 1 HDTHETAt  3 1 

00002030 

LDTHI 3 )=OTHPRE(l >*DTHSTA< 2J-DTHPREI 2 )*DTHETAt 1 » 

00002040 

UELT2(  1,2  )=-LO Till  1) 

00002050 

tOELTSI  1 >3  )=»LDTil(  2 ) 

00002060 

LDELT2<  l ,4  )=-LOTH(  3 ) 

00002070 

LCELT2I2.1  ) = L0TH(1» 

00002000 

LDELT2I 2 >3  )*LOTN(  3 ) 

00002090 

I.DELT21 2*4  )=-LQTH(  2 i 

00002100 

LQSLTCI 3,1 )= LOTH ( 2 ) 

00002110 

LDELTCI  3,2 )B-LOTHt  31 

0000  2120 

LDCLT2t3.4)=L0THIH 

00002130 

L0ELT£<4,l»al3TMtS» 

00002140 

10ELT2C  4,2  J=IDTH( 2 ) 

00002150 

L0ELT2(4,3)=»L0THU) 

00002160 

CCBl. ♦!./«. *U0TH2 

00002170 

C3sl./2.*1./49.«U0TH2 

00002100 

* 

00002190 

00  180  16*1,4 

00002200 

QTENPi  16  l=C2*QUAT(  16 ) 

00002210 

• 

4# 

00002220 

00  100  J6Qi*4 

00002230 

IF  U6.EQ.I6)  GOTO  180 

00002240 

QTEHPt  16 1 «<  C3»U0E  Lf(  16 . J6  >*10ELT2«  Xb , J6  >/24.  »*QUATt  J6  WQTEHPt  16  > 

00002250 

ISO 

CONTINUE 

00002260 

c 

00002270 

00  1«0  17*1.9 

00002260 

Q’JAt<17»*QTEHP(I7> 

00002290 

190 

CONTINUE 

00002300 

c 

00002310 

C EK3  QUATERNION  CALCULATION 

00002320 

C 

00002330 

200 

CONTINUE 

00002140 

C NORNAlUE  QUATERNION 

00002350 

IF  O.  IX,  THORN*.  001 1GOTO  900 

00002360 

OsQUATt  1 l*QUAT«  1 ^♦QUAT^^1*CUAT^2  ><QUATO)«QUATC3> 

00002370 

A *QUAT(9l**QtiATt4l 

00002380 

IF  I0.EQ.1, » GOTO  900 

00002390 

01*509X1 0> 

00002400 
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C 

DO  050  J9=l>9 
050  QUAT(J9I=QUAT<J9J/D1 
C 

TNORM»T*DTNRH 
900  CONTINUE 

CONVERSION  FROM  QUATERNION  TO  OIRECTION  COSINE  MATRIX 

QlSsQUATC 2 IkQUATI 2 > 

Q2S=QUAT( 3)*QUAT( 3 ) 

Q35SQUAT( 9 )#QUAT( 9 ) 

Q01=QUAT( 1 )**QUAT(  8 I 
Q02-QUATI 1 )*QUATt  3) 

Q03>QUAT(  X )**QUAT  ( 9 ) 

Q12=QUATI2>*aUAT(3> 

Q23aQUAT(  3)t*QUAT(9) 

Q3IsQUATt  9 MQUATt  2 ) 

0CM( 1 1*1 . 0-2 . 0*1 Q2S+Q3S i 
0CH12)52.0*(QI2-Q03> 

OCHt 3 )=2 , 0*( Q31 *QQ2 ) 

DCHI 9 1=2 . 0# I Q12+Q03 I 
DCM<SI=1.0>2.0*(Q3S+Q1S1 
DCM(6)s2.0W(Q23.QQU 
0CN(  7)=2,0*»cQ3I*Q02 ) 

OCH(01=2.O*.QC3*QO1> 

OCMI9)sl,0'2.0M|QIS«Q2S> 

GO  TO  700 

600  CONTINUE 

DIRECTION  COSINE  MATRIX  UPDATE  CALCULATION 
IF  (N0RDE8.LT.  II  60  TO  300 
FIRST  ORDER  UPDATE 

ornm«  1.0 

0THISI*  1.0 
OYM(9»a  1.0 
OTMI 2 l*"0THSTA( SI 
0THI 31*  OTHETA(E) 

0YMI9!*  DTHITAHI 
d THt  6 l*«0TMETA( 1 1 
OTMt  T >»<OTHETA( 2 1 
OTHI0I*  DTHtTAIll 

SECOND  ORDER  UPDATE 

If  IMOROIR  .IT,  21  60  TO  620 
LOTHmsOTMETAlllM* 

LOTH!  2 IsOTHCTAI  2 U»»2 
U5TH«31*0THE1Al31*»2 
DTHt  l l*0TMt  1 >‘6. 5*(  LOWt  2 »*U0THt2m 
OTMI  S)*OTHt  S 1-0  .S»U0TH(  3 > ♦LOTHll  I » 

OTHI  9!*0THI  9l-O.S»UOTHm»LOTH(  6 1 > 

6THI 6 IsDTMl  2 1 *0 . 5*0TNETAC  1 UBTHETAI 2 » 

OTMI  3>*0TMI  3i»0,5»OTHETA(  3 l»OTHETAm 
DtMl 6l*0Trtl6 1*0. 5«OTHETA( 2 J*0THETA( 3 » 


00002910 

00002920 

00002930 

00002990 

00002950 

00002960 

00002970 

00002900 

00002990 

00002500 

00002510 

00002520 

00002530 

00002590 

00002550 

00002560 

00002570 

00002500 

00002590 

00002600 

00002610 

00002620 

00002630 

00002690 

00002650 

00002660 

00002670 

00002690 

00002690 

00002700 

00002710 

00002720 

00002730 

00002790 

00022750 

00002760 

00002770 

00002700 

00002790 

00002000 

00002010 

00002020 

00002030 

00002090 

00002650 

00002660 

00062070 

00002000 

00002090 

00002900 

00002910 

00002920 

00002930 

00002990 

00002950 

00002960 

00002970 

00602906 

00002990 

00003000 
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DTMI 4 )=0TM( 4 )»0.5#DTH£TA(  1 )#0THETA( 2 ) 

DTMI 7 J=0TMl 7 )+0.5#0THETA( 3 )#0THETAt 1 ) 

DTMI 8 )=DTHt 0)+0.5#DTHETA< Z )#QTHETA< 3 ) 

620  CONTINUE 

THIRO  ORDER  UPOATE 

IF  t HORDES  .IT.  3)  GO  TO  630 

0=0,.  16666666666666# ( LDTH(  I JHDTH1 2 ) + L0THl  3) ) 

THIRD  ORDER  TERNS  SET  TO  ZERO  DUE  TO  PROFGEN  INFINITE  ANGULAR  ACCEL 
C1=0.0 
C2=0.0 
C3S0.0 

EXPRESSIONS  FOR  THIRO  ORDER  TERMS 

Cl=l  DTHETAI 3 )#DTHPRE( 2 )-DTHETA( 2 )#DTHPRE< 3 ) )#0. 63333333333333 
C2=<  -DTHETAI  3 )#DTHPREtl  J+DTHETAI  1 HDTHPREI 3 ) »#0. 63333333333333 
C3M  DTHETAI 2 >*0THPRE(  1 1-0THETAI 1 )*DTNPRE(2  > >#0.63333333333333 

OTMt 2 J=0TMI 2 )*0*DTHETA< 3 )-C3 
DTMI 4)=0TM(  6 I'D*  DTHETAI  3 )*C3 
DTMI 3)=DTM( 3 )'0#DTHSTA< 2 >fC2 
DTMI  7)=0TN<  7UD«0THETA(  2 1-C2 
DTMI 6 )SDTN( 6 )*D#DTHETA(  1 )-Cl 
DTMI  0 l=DTM(6)-0#DTHETAI  l)*Cl 
630  CONTINUE 

IF  (N030ER  .GT.3)  GO  TO  300 

992  FORMAT  1 18X>6H  TEST  ,3GI6.d./26X,3G16.6»/26X,3G16.6.//> 

MITE  I OF  I LE  .992  )QTM 
NRITE  (0FXLE,9'J2IQI0 
CALL  NXMI  QItJ.OTMiOCM  I 
KRITE  IOFILE.992 10CM 

NORMALIZE  DIRECTION  COSINE  MATRIX 

IF  IT  ,LT.  TNORM'.OOU  CO  TO  700 
DO  710  11=1,9 

OTEMinjsOCMCin 
710  CONTINUE 

CALL  MTXHI  OTEHtOCM, ATEH > 

ATEMt 1 J»ATCMI 1 )-1.0 
ATEMI5>=ATi:MI5>-1.0 
ATEMI 9>«ATEMt9t“i.O 
CALL  MXMieCM.ATIM,OTEM> 

DO  720  11=1,9 

OCMI U UOCMI Xll“O.S»OTEM!Il) 

720  CONTINUE 

TN'OTMsT.OTNRM 
700  CONTINUE 

DO  730  Sin  ,9 
o;o«n>*Dc«m> 

730  CONTINUE 


00003010 

00003020 

00003030 

00003060 

00003050 

00003060 

00003070 

00003060 

0000309C 

00003100 

00003110 

00003120 

00003130 

00003160 

00003150 

00003160 

00003170 

00003160 

00003190 

00003200 

00003213 

00003220 

00003230 

00003260 

00003250 

00003260 

00003270 

00003260 

00003290 

00003300 

00003310 

00003320 

00003330 

000033'iQ 

00003350 

00003360 

00003370 

00003360 

00003390 

00003600 

00003410 

00003420 

00003430 

00003440 

00003650 

00003460 

00003470 

00003400 

00003490 

00003530 

00003510 

00003520 

00003530 


END  OF  DCH  UPOATE  00803540 

00C03S50 

COO  CONTINUE  00003560 

MITE  IO?ILE,992)OCM  03003370 

00093500 

END  Of  QUATERNION  OR  COM  UPDATE  00003550 

00003600 


non  oooofln  o u o o n o n o o no 


FORM  APPROXIMATE  MIDPOINT  OCM,  OCMMIO 
00  192  Kl=l, 9 

0CKMI0I K1 )s0.5*(0CM(Kl )*OCMOLO(K1 ) ) 
.92  CONTINUE 

CALCULATE  ROTATED  DV 

CALL  MXV(0CMMI0,DV,0VI) 

END  DV  CALCULATION 

DVN( 1 )=0VN( 1 )+DVI( 3) 

0VN(  2 )~DVN(  2 ) +D  VI 1 1 ) 

OVNt  3 )=DVNt  3 ) +DVK  2 ) 

SWET=SIN(WE*T) 

CWETsC0S(WE*T) 

DO  191  K9S1,9 
DTEM( K9 ) =DCt1(  K9 ) 

,91  CONTINUE 
TFM(l)=SWET 
TFNC 2)50.0 
TFK(3)=CWET 
TFM(9)sCWET 
TFM(S)50.0 
TFM(6 Ib-SWET 
TTMt 7)50.0 
TFma)=1.0 
TfS1(9)*0,0 

CALL  MXN( TFM ,0TEH,0CM ) 

WRITE  (OPILE,992)DCM 


DCM  IS  BODY  TO  EARTH  FIXED  TRANSFORMATION 


XF(KOROER.NE.3)  GO  TO  950 
0THP3EI 1 )=DTHETA( 1 ) 

OTHFSE( 2 la0TM6TAt  2 ) 

OtHPREl S l*OTHETAt  3) 

9S0  CONTINUE 

OUTPUT  AND  PRINT  CONTROL 

I F IPCNTOr.GT.O)  GO  TO  960 
IF  (HODPOT.EO.Ot  CO  TO  999 
C 

IF  (T.LT.TPCMC0-.O80SI  GO  TO  999 

YPRrtOO«rPRHOO*HGOPOT 

GO  TO  970 

c 

960  CONTINUE 

IF  ( T < IT . TPRNT « , 0005)  GO  TO  999 
TPRNTaTPRNT«PtfNTOt 
C 

970  CONTINUE 

IF  (PflNTSN.LT.l.)  GO  TO  999 


00003610 

00003620 

00003630 

00003640 

00003650 

00003660 

00003670 

00003600 

00003690 

00003700 

00003710 

00003720 

00003730 

00003740 

00003750 

00003760 

00003770 

00003700 

00003790 

00003600 

00003610 

00003820 

00003630 

00003640 

00003650 

00003660 

00003870 

00003660 

00003890 

00003903 

00003910 

00003920 

90003930 

00003940 

00003950 

00003960 

00003970 

00003980 

00003990 

00004000 

00004010 

00004020 

00004030 

00004040 

09004050 

00004060 

00004070 

00004000 

00004090 

00004100 

00004110 

00004120 

00004130 

00004140 

00004150 

00004160 

00004170 

00004100 

00004190 

00004200 
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WHITE  COFILE, 1300)  OVN 
IF  (NGOCN  .EH.  0)  WRITE 
WRITE  COFXU.14QO)  DCM 


(OFILE.1SOO)  QUA? 


999  CONTINUE 

TALG=T  ♦ DT 
RETURM- 


ERROR  RETURN  FROH  At©  MODULE 

jjO  FoSIt3(3X, "ORDER  NOT  VROMSRW  SPECIFIED") 

IENDF=I  . 

RETURN 


ALGORITHM  INITIALIZATION 

500  CONTINUE 
REWIND  IFtLI  . 

501  READ  UFILEaOOOM^DATAMK) 

IF  (EOF(IFILE))  502*501 

5C2  CONTINUE 

503  READ! FFILE. 1000)  IX.PDATACIX) 

IF  IEOF1PFILE))  509,503 
509  CONTINUE 

REWIND  FFILE 
REWIND  IFILE 
OFILEsXPILE 

oo  sol  m-i.s 
ovmuD^f  o 
OTHP3eiUij*“0.0 
521  CONTINUE 

TNC-S?rtsT*OTN8M 

N.T3tS«(lt'ffiJC» 

,N3SDER«0i!CEI? 

C05F=C03iPiTCH»PIT£Rft  /P9T800) 
COOV*C»3I  VAH+TAWtRR^ROTCEa ) 
COSR*C03t  t}0LL*«OLERR/R0TS06  S 
COSUTfeCOSl  HAT  ♦LATIRR/HOTCOS ) 
CaSieN*C03!  I«.CN»  LCNS^fi^OTOOO  ) 
C0SVssL93)WAN3ER*Wf.NERR^F0T000 ) 
OIN^5INfPlTCM*P?TEW^OT090) 
SSNV=$tN«VAW*VAWSW'POTC  1)5) 
SI»{9sSINi  R0U,«R0tEStV&0TC2G 1 
SlNLAfaSDN  HAT*UfERS/ROT&nS) 

StiSLONsJIHI  ItttNUWtRR/RDTCCG) 
51NU-i$INt«AN3£9»NMlE9B/D0T(SCSI 
<H'S«l)»CfSF«3INV 

«i>{  ?)s*5IN»«M»*»w3I»T>'C05V,'C«W 
tjFOi  3l**C03R*SI«?*aiNTie05V*SUS 
uFD«s,i*tOfiV>*COSV 

UF31 $ }*-SlKC*SjNF»C05V*C0SS^Dn 
^&J6ls-C0SR4BlKP*COSV»$5Jv34§tNir 
CP3C7>sS’Ni* 

^lOteCOE-i^StNR 

GFOl9»6C05P<*SOTiR, 


00004210 
00004220 
00004230 
00004240 
38QQ4250 
00004260 
00004270 
00004260 
00004290 
’ 00004300 

00004310 
00004320 
00004330 
00004340 
00004350 
000043*0 
00004370 
00004360 
00004390 
00004900 
00004410 
00004420 
00094430 
00004440 
00004450 
00004460 
C0004470 
00004430 
00004490 
00084500 
OQ0Q4S1O 
00004520 
00004539 
00144540 

80094550 
’ . ' 00004500 
OOfliiOSTO 
■ ULS'JifSO 
8OO04S90 
O090460O 
59009$1S 
03064620 
-•■■■  03604630 

00004649 
099Q46SO 
00009660 
00004670 
00089600 

eOOO'V>98 

0CC09  700 
00639710 
Of 634723 
00034730 
00009749 
09OO67SO 
90009769 
• 00006770 
00006700 
93904760 
00606000 


QISM  X )=COSlC^COSM"S;KlCf^Stt«*SINUT 

(JIP<  2 )=-SIKM«*COSlW!-COSM»SXNWW»SINUT 

QIP(3)sC0SUT*SINl.O‘4 

W<4>sSINWttC0SWT 

QIPf6)=C0$U*CQSEAT 

Q:Pt6t»$INUT 

G1P(  7 ) =-COSW*SIM4.CN-8IHM*SXMlftT«CO§iCM 
QIP<0)sSINW»SIKtOU-SINUT«COSieN*CGS« 

QIPl9)iC0SLAT«C0SL0M 
CALL  MXMIQIP.QPS.QXB) 

WRITE  (OFUE,  993)010 

993  FOKHATl"  QIB=‘,,AX,3F15.a,2C/,6X.3F15.8 J » 

QU4T(  1 JsSQRTC  ( 1 . +QID(  1 )*QIBt  5)+QIBt9) )/4.  ) 
QUA7(2)=(QIB<a)-QIBf6))/(4.>4GUATU)) 

QUATf  3 )=<  QIBl  3 >-qiB(  7 ) >/(  A .WJUATtl > > 

QUAT(4)  = (QI0(4  )-QIB(  2)  )/(4.*QUAY(l) ) 

C 

IF  f N90CH  ,EQ.  OX  60  TO  995 
00  994  UsX, 4 

cuATin»=o.o 

994  CONTINUE 

995  CONTINUE 
C 

WRITE  (OFILEtXOXOI  OT.PRNTSW.OUTSM.OFUE.COHOCM.MlJOPOT.PflmOT, 
X QUAT.NWJES  .OTtiRH,  UTEBR , LCN6S8 » WANEHtt , PITERS tRGLEHR jYAUEHR 
C 

00  53*  13*1,5 
OTHPU5>=0. 

S31  CONTINUE 
C 


IHITSun 

TAt6=T*0Y 

RETURN 


1000  FORMAT 
XOXO  FORMAT! 
x/sx.eH 

X/3X.6H 
X/3S,CN 
X/3X.&H 
X/ SK.6H 
X/3X.6N 
X/3K.6N 
X/3X.6M 
K/SX.OH 

x/ix.eH 

X/JX,CM 

X/SX.6H 

X/3R.C?* 

X/3X,C4I 

X/SX.8N 


4I5.F20.XC1 

30H  ALGORITHM  INITIALISATION 
ot  ,3:<«su.e,sx,4»  sec. 
PRHYSH  ,?X.OX6,0, 

(H1YSW  .3X.CU.0. 

OFiie  ,>x,m» 

QGPUCH  ,3X,Gi6.8, 

Hooper  ,3X,ei8,«. 

PfiNTOY  ,JH.C16.3, 

BOAT  ,4ilX, 6X8.0), 

ORDER  ,3X,1$, 

OTNSM  ox,  0X4.0, 

UTERR  , 3X.G16.8.4H  DEG, 
E0NIRR  ,3X,ei4.0,4H  DIO, 
WAKES?)  ,3X,C16.e,4H  PEG, 
PXTERR  ,3X,f»6.0,4H  CEO. 
R0LEK3  .SX.OXi.fl.W  CEO, 
YM1ERR  ,3X, 6X4. 6,4(1  DEG./) 


1204  FORMAT!  6X.25H  »*  AEG  *«  DVmr/SEC)  ,3Olft.0./l 
1380  FOTHAYlftX.JSH  **  AEG  ««•  OVWFY/SECt  .3G160./T 
1400  rC3MATI  X5N,tll  OEM  ,3GX6.S,/24X, 3016. 0,/iftX, 301ft. 0.//) 
1500  FORMAT! 16X.6H  QUAY  ,4616.8,//) 

END 


00004010 

00004620 

00804030 

00004640 

00004850 

00004860 

00004670 

00004080 

03004890 

00004900 

00004910 

00004920 

00004930 

00004940 

00004950 

00004960 

C0004970 

00004930 

00004990 

00005000 

00005010 

00005020 

00005030 

00005040 

00005050 

00Q0S060 

00005070 

00005080 

00005090 

00005100 

00005110 

00005120 

00005130 

00005140 

00065150 

00005160 

00005170 

00005160 

00005190 

00005200 

00005210 

00005220 

0000§539 

00005240 

00005250 

OOOOSfftO 

00005270 

00005630 

00005590 

00005300 

00005310 

00305320 

00905330 

00005349 

C000S350 

40005360 

00005370 

00005380 

06005390 
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oooonooo 


00000010 


05/10/78  DATE  OF  CURRENT  MODULE 

00000080 

00000050 

THE  LOCAL  LEVEL  NAVIGATION  MODULE  TRANSFORMS  THE  INCREMENTAL 

00000040 

VELOCITIES  TO  LOCAL  LEVEL  HANDER  AZIMUTH  COORDINATES  AND  COMPUTES 

00000050 

THE  POSITION,  VELOCITY,  AND  ATTITUDE  IN  THIS  FRAME  USING  ALGORITHMS 

00000060 

SIMILAR 

TO  THE  UPGRADED  NUMSIM  ALGORITHMS  CR-977.V0L  II, SEC  8. *.4) 

00000070 

QOOOOOSO 

SUBROUTINE  LLN  (T.IENOF ,DVI,ALTO,DCM, 

00000090 

X 

NAVLAT , NAV  LON , NAVV , NAVH i NAVP , NAVR , NAVHO ) 

00000100 

ooooouo 

REAL 

AO, 3) 

00000120 

REAL 

ALP 

00000130 

REAL 

ALTERR 

00000140 

REAL 

ALTO 

00000150 

REAL 

ATA( 3,3 ) 

00000160 

REAL 

ATEMPI3.3) 

00000170 

REAL 

CALF 

00000180 

REAL 

CGDL 

00000190 

REAL 

CCSLON 

00000200 

REAL 

COSLT 

00000210 

REAL 

CWET 

00000220 

REAL 

DATA(IS) 

00000230 

REAL 

DROLL 

00000240 

REAL 

OPITCH 

00000250 

REAL 

OYAW 

00000260 

REAL 

DVC( 3) 

00000270 

REAL 

OVE(  l ) 

00000280 

REAL 

DVK  3) 

00000290 

REAL 

DT 

00000300 

REAL 

G 

00000310 

REAL 

GRC3) 

00000320 

REAL 

IAX 

00000330 

REAL 

IAY 

00000340 

REAL 

IAZ 

000003S0 

. REAL 

ILAT 

00000360 

REAL 

ILON 

00000370 

REAL 

IH 

00000380 

REAL 

INAVLA 

00000390 

REAL 

INAVLO 

00000400 

REAL 

ITEMPI 

00000410 

REAL 

ITEMF2 

00000420 

REAL 

ITEMP3 

00000630 

REAL 

ITEMP4 

00000440 

REAL 

ITEMP5 

00000450 

REAL 

IVC  3) 

00000460 

REAL 

LATERR 

00000470 

REAL 

LONEKR 

00000480 

REAL 

MODFDT 

00000490 

REAL 

NAVH 

00000500 

REAL 

NAVLAT 

00000510 

REAL 

NAVLON 

00000520 

REAL 

NAVV( 3 ) 

0090C530 

REAL 

P0ATAI20) 

00000540 

REAL 

PITCH 

00000550 

REAL 

PRHTDT 

00000560 

REAL 

RE 

00000570 

REAL 

R0T00G 

00000500 

REAL 

RNOI3) 

00000590 

RIAL 

ROIL 

00000600 
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REAL  SAIF 

00000610 

00000620 

REAL  SPAREX 

00000630 

REAL  SPARE2 

00000640 

REAL  SUET 

00000650 

REAL  SINLON 

00000660 

REAL  SINLT 

00000670 

REAL  THG( 3 ) 

00000600 

real  tprmod 

00000690 

REAL  TPRNT 

00000700 

REAL  V(3) 

00000710 

REAL  VERR13) 

00000720 

REAL  ME 

00000730 

REAL  WET 

00000740 

real  WONDER 

00000750 

REAL  WXV( 3) 

00000760 

REAL  YAW 

00000770 

REAL  NAVP 

00090760 

00000790 

REAL  NAVR 

00000600 

REAL  NAVHO 

00000610 

REAL  OEN 

00000620 

REAL  OCH(9) 

00000830 

REAL  AV(9) 

00000840 

REAL  OTE!i(  9) 

00000850 

REAL  OV(3) 

00000660 

REAL  HV(  3) 

00000870 

REAL  XV( 3) 

00000660 

REAL  OELH 

00000890 

REAL  CVD1 

00000900 

REAL  CVD2 

00000910 

REAL  CV03 

00000920 

REAL  VCHP 

00000930 

REAL  SSL AT 

00000940 

REAL  xsslat 

00000950 

REAL  XCALF 

00000960 

REAL  xsalf 

00000970 

REAL  THY 

00000980 

REAL  THZ 

00000990 

REAL  M20T( 3i3) 

00001000 

REAL  t12DT02(3»3J 

00001010 

REAL  XA13»3) 

00001020 

REAL  H 

00001030 

PEAL  H8 

00001040 

REAL  OHB 

00001050 

REAL  XHB 

00001060 

REAL  oalf 

00001070 

real  oalf 

00001080 

INTEGER  NORTH 

00001090 

00001100 

integer  nup 

00001110 

INTEGER  PFILE 

00001120 

INTEGER  OFILE 

00001130 

EQUIVALENCE  (OATA(l)»  ot> 

00001140 

00001150 

EQUIVALENCE  tOATA<2)«  PFNTSW) 

00001160 

EQUIVALENCE  (0ATA(5)c  OUTSW 

00001170 

EQUIVALENCE  <DATA(4>,  XFILEJ 

00001180 

EQUIVALENCE  <0ATA<5)»  SPARE!) 

00001190 

EQUIVALENCE  C PATAC 6 ) * MOOPOT) 

00001200 

EQUIVALENCE  ( OATA( 7 ) . ALTERR ) 

■ 

ono  o o o o 


c 


c 


c 

c 


EQUIVALENCE  <DATA(8),VERR(1)) 
EQUIVALENCE  (OATAt II  ),LATERR ) 
EQUIVALENCE  <DATA(12 ) . LONERR ) 
EQl  :VALENCE  (DATA(13),CVD1) 
E5t  IVALENCE  (DATAt 14)  ,CVD2 ) 
EQUIVALENCE  <DATA(15),CV03) 


EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 

EQUIVALENCE 


( PDATAt  1),  WE) 
(PDATA(2)>  RE) 
(PDATA13).  G) 
(PDATA(4),  PRNTDT) 
( PDATAt  5 ) > ILAT) 

( PDATAt 6 )i  I LON) 
(PDATAt  7),  WONDER) 
(PDATAt  8),  IH) 
(PDATA(9)i  ROLL) 
(FDATA(IO), PITCH) 
(PDATAt 11), YAW) 
(PDATAt 12), DROLL) 

( PDATAt 13) tDPITCH ) 
( PDATA( 14 ) »0YAW> 

< PDATA( IS) )IV( 1 ) ) 

( PDATAt 16 ) tIVt  2 ) ) 

( PDATAt 1?) ,IV( 3 ) ) 

( PDATA( 18) ,IAX) 
(PDATAt 19), IAY) 
(POATAt 20 ),IA2) 


DATA  RDTODG  /57. 29577951/ 
DATA  TPRNOD/O . 0/ 

DATA  TPRNT/0.0/’ 

DATA  IFILE/60/ 

DATA  INIT3W/0/ 

DATA  PFILE  / 7 / 


IF  (IENDF.EQ.1)  RETURN 
IF  ( INITSW.EQ.O ) GO  TO  500 
IF  (T.LT.TLLN-.0005)  RETURN 


TRANSFORM  FROM  INERTIAL  TO  EAR INFIXED  (AT  MID  COMP  CYCLE) 

WET=NE*(T-0.5#DT) 

$WET=3IN(WET) 

CWET=COS( WET ) 

DVE(  1 )=CWET*DVI(  1 )+SWET*0VI(  2 ) 

DVE( 2 )=-SWET*DVI< 1 >+CWET*0VI( 2 ) 

0VE(3)=QVI<3) 

TRANSFORM  FROM  EARTHFIXED  TO  COMPUTATIONAL  (MID-COMP  CYCLE) 

DO  10  11=1,3 

0VC(I1)=  XA(1,I1)  * OVE(i)*  XA(2,I1)  » 0VE(2)A  XA(3,I1)  » DVE(3) 
10  CONTINUE 
C 

C UPDATE  VELOCITY  (UEN  FOR  ALF=0) 

C 

DO  20  12=1,3 
OV( 12  )=V( 12 ) 


00001210 

00001220 

00001230 

00001240 

00001250 

00001260 

Q00J1270 

00001280 

00001290 

00001300 

00001310 

00001320 

00001330 

00001340 

00001350 

00001360 

00001370 

00001380 

00001390 

00001400 

00001410 

00001420 

00001430 

00001440 

00001450 

00001460 

00001470 

00001480 

00001490 

00001500 

00001510 

00001520 

00001530 

00001540 

00001550 

00001560 

00001570 

00001580 

00001590 

00001600 

00001610 

00001620 

00001630 

00001640 

00001650 

00001660 

00001670 

00001680 

00001690 

00001700 

00001710 

00001720 

00001730 

00001740 

00001750 

00001760 

00001770 

00001780 

00001790 

00001800 
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V( 12 )=V< 12 )*DVC( 12  J+WXVC 12 )+GR( 12 1#0T 
20  CONTINUE 
C 

C AOO  VERTICAL  DAMPING  TERM 
C 

V(  1 ) =VU  )♦  ( V0MP+CVD2#DE  LH  )*DT 
C 

C AVERAGE  OLD  A NEW  V(I)  TO  OBTAIN  INTERPOLATED  VALUE,  HV(I) 

C AND  EXTRAPOLATE  TO  OBTAIN  XV(I> 

C 

DO  25  1=1,3 

XVtl)  = 1.5#V( I ) > 0.5#OV(I) 

KV(  I ) = 0.5#( V<  I ) ♦ OV(  I ) ) 

£5  CONTINUE 
C 

C COMPUTE  ANGULAR  VELOCITY  C RHO 1 
C 

CALL  ANGVE L( HV ,XCALF , XSA LF , XSSLAT , XK »RHO ) 

C WRITE  (OFILE.551)  RHO 

C 

C UPDATE  TRANSFORMATION  FROM  COMPUTATIONAL  TO  EARTH  FIXED 
C 

THY  = RHO(2)#DT 
THZ  = RHOt  3)#DT 
CALL  AUP( THY , THZ ,M2DT ) 

C WRITE  ( OFILE.551 ) H2DT 

CALL  MM( A ,M20T , ATEMP ) 

C 

DO  AO  M3=I,3 

DO  AO  MA=1,3 

AO  A(M3,MA)=ATEMP(M3,MA) 

C WRITE  (OFILE.5S1)  A 

NUP  = NUP  ♦ 1 

C 

C IS  IT  TIME  TO  ORTHONORMALIZE  A? 

C 

IF  (NUP  ,LT,  NORTH ) GO  TO  60 
NUP=0 
C 

C ORTHONQRMALIZE  TRANSFORMATION 
C 

DO  50  IA=1 ,3 
DO  50  I5=IA,3 

ATA( IA,I5)=A(  1,IA)*A(  1»I5)*A(2,IA)*A(  2,I5)+A(  3,IA)NA(  3,15 > 
ATA( I5,IA)=ATA( IA,I5) 

50  CONTINUE 

C 

C WRITE  (OFILE.551)  ATA 

ATA(1,1)5ATA(1,1)-1. 

ATA(2,2)=ATA(2,2)-1. 

ATA(3,3)=ATA(3,3)-1. 

C 

DO  70  16=1,3 
DO  70  17=1,3 

ATEMPI  I6,I7)=A( 16,17)*. 5#tA(  16.1 )«ATA( 1,17) 

X *At 16,2 )#ATA( 2,17 )*A< 16, 3)#ATA( 3,17) ) 

70  CONTINUE 

60  CONTINUE 


RESTORE  MATRIX  A ROW  WISE  AS  VECTOR  AV 


30001610 

00001820 

00001830 

000016A0 

00001650 

00001660 

00001C70 

00001680 

00001690 

00001900 

00001910 

00001920 

00001930 

000019A0 

00001950 

00001960 

00001970 

00001960 

00001990 

00002000 

00002010 

00002020 

00002030 

Q00020A0 

00002050 

00002060 

00002070 

00002080 

00002090 

00002100 

00002110 

00002120 

00002130 

OOOOEIAO 

00002150 

00002160 

00002170 

00002100 

00002190 

00032200 

00002210 

00002220 

00002230 

00002240 

00002250 

00002260 

00002270 

00002260 

00002290 

00002300 

00002310 

00002320 

00002330 

0Q0023A0 

00002350 

00002360 

00002370 

00002380 

00002390 

00002A06 


c 

11=0 

DO  90  10=1,3 
00  90  19=1,3 
11=11*1 

AQ8,I9)=ATEMP(  10,19) 

AV( II  )=A( 10,19) 

90  CONTINUE 

C 

C WRITE  (OFILE, 551)  A 

C 

C EXTRAPOLATE  A TO  MID  COHP  CYCLE 
C 

TKY=0.5#THY 

THZ=0.5#THZ 

CALL  AUP( THY, THE, MED TOE) 

C WRITE  ( OFILE >551 ) MEOTOS 

CALL  MN(A,M20T02,XA) 

C WRITE  (OFILE ,551 ) XA 

C 

C VERTICAL  OAMPING  CALCULATIONS  INCLUDING  EXTRAPOLATION 
C 

H = H ♦ (HV( 1 ) + CVD1*DELH)*DT 
XH  = H * 0.5#V(1)*DT 
HB=ALTO 

XH3  = 1.5»H3  - O.S*OHB 
OELH  = XH3  - XH 
OH0  = H3 

VDMP  s VOMP  + CV03#0ELH»0T 
NAVH=H 
C 

C EXTRACT  LAT,  LON,  ALF,  EXTRAPOLATE  ALF 
C 

CGOL=DSQRT(  A<1 . 1 )*A(  1 , 1 )*A(  E , 1 )«At  S , 1 ) ) 

NAVLAT=DATANE( A( 3,1 ) tCGDL) 

OALF  s ALF 

IFtCGOL  . EQ.  0. ) GOTO  115 
NAVLOH=DATANS(A(E,l),A(l,l)) 

ALF=DATAN£(  A( 3,E ) ,A( 3.3) ) 

DALF  s 0.5#( ALF-QALF ) 

115  CALF=DCOSt ALF ) 

SALF=DSIN( ALF ) 

XSALF  = SALFV  DALF*CALF 
XCALF  = CALF-  0ALF*SALF 
C 

C TRANSFORM  VELOCITY  TO  ENU 
C 

NAVVt  1 )=CALF#V<  2 )-SALF*V(  3 ) 

NAVVt  £ )=  SALF#V( S ) +CALF**Vt  3 ) 

NAVV( 3)SV< 1 ) 

C 

C COMPUTE  GRAVITY  GR,  EXTRAPOLATED  TO  MID  COMP  CYCLE 
C 

CALL  GR AV ( XA , XH , XSS LAT , GR ) 

C WRITE  (OFILE, 551)  GR 

CALL  ANGVE L ( XV , XCA LF , X3A LF , XSSLAT , XH , RHO ) 

C WRITE  (OFILE, 551)  RHO 

C 

C COMPUTE  COMP  TO  INERTIAL  RATES  X OT  AND  CORIOLIS  CORRECTIONS 
C 


00002410 

00002420 

00002430 

00002440 

00002450 

00002460 

00002470 

00002400 

00002490 

00002500 

00002510 

00002520 

00002530 

00002540 

00002550 

00002560 

00002570 

00002500 

00002590 

00002600 

00002610 

00002620 

00002630 

00002640 

00002650 

00002660 

00002670 

00002600 

00002690 

00002700 

00002710 

00002720 

00002730 

00002740 

00002750 

00002760 

00002770 

00002700 

00002790 

00002000 

00002610 

00002020 

00002030 

00002040 

00002050 

00002660 

00002070 

00002000 

00002090 

00002900 

00002910 

00002920 

00002930 

00002940 

00002950 

00002960 

00002970 

00002900 

00002990 

00003000 
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CALL  TORCOR(XA,RHO,XV,DT,THG,WXV) 

WRITE  <0FILE,551)  THG 
WRITE  (0FILE>551)  WXV 

ZERO  OVI 

DO  140  J4=l»3 
DVI( J4)=0. 

40  CONTINUE 

COMPUTE  BODY  TO  COMP  FRAME  TRANSFORMATION  (ATTITUDE  MATRIX) 
AND  EXTRACT  ATTITUOE  ANGLES 

CALL  MTXM( AV1DCM1DTEM) 

DEN  = SQRT( DTEM( 2 )#*2+0TEM( 3 )##2 ) 

IF  (DEN  .EG.  0.0)  GO  TO  950 
NAVP  = ATAN2(DTEM( 1 ) .DEN ) 

NAVR  = ATAN2(DTEM( 2 ) iDTEMt  3 ) ) 

NAVHD  = ATAN2 ( DTEHt 4 ) t DTEM( 7 ) ) - ALF 
GO  TO  940 

950  WRITE  (OFILE.1250) 

TLLN=T+DT 
RETURN 
940  CONTINUE 

OUTPUT  AND  PRINT  CONTROL 

IF  (PRNTOT.GT.O)  GO  TO  960 
IF  (MOOPDT.EO.OJ  GO  TO  999 

IF  (T.LT.TPRMOD-.OOOS)  GO  TO  999 

TPRMOD=TPRMQD+NOOPDT 

GO  TO  970 

960  CONTINUE 

IF  (T.LT.TPRNT-,0005)  GO  TO  999 
TPHNT=TPRNT+PRNTDT 

970  CONTINUE 

IFtPRNTSW.LT.l. ) GOTO  999 

I7EMP1=NAVLAT*RDT0DG 
• ITEMP2sNAVLON«RDTODG 
ITEMP3=NAVP  *t  RDTOOG 
ITEMP4=NAVR  P RDTODG 
ITEMP5=NAVHD  PRDTODG 


WRITE  (OFILE.X200)  NAVH.NAW,ITEMP1,ITEMP2»ITENP3,ITEMP4.ITEMP5 
WRITE  (QFXLEil225)  A 
WRITE  (OFILE.1235)OTEM 


999  CONTINUE 
TLLN=T+OT 
RETURN 

NAVIGATION  MODULE  INITIALIZATION 
500  CONTINUE 


00003010 

00003020 

00003030 

00003040 

00003050 

00003060 

00003070 

00003060 

00003090 

00003100 

00003110 

00003120 

00003130 

00003140 

00003150 

00003160 

00003170 

00003160 

00003190 

00003200 

00003210 

00003220 

00003230 

00003240 

00003250 

00003260 

00003270 

00003260 

00003290 

00003300 

00003310 

00003320 

00003330 

00003340 

00003350 

00003360 

00003370 

00003380 

00003390 

00003400 

00003410 

00003420 

00003430 

00003440 

00003450 

00003460 

00003470 

00003460 

00003490 

00003500 

00003510 

00003520 

00003530 

00003540 

00003550 

00003560 

00003570 

00003580 

00003590 

00003600 


oooo  o oono  on  o n no 


REWIND  IFILE 
REWIND  PFILE 

501  READ  (IFILE. 1000)  IX.DATA(IX) 

IF  (EOFt IFILE))  502,501 

502  CONTINUE 

503  READ  ( PFILE.1000)  IX.PDATAUX) 

IF  (EOF(PFILE))  510,503 

510  CONTINUE 
REWIND  IFILE 
REWIND  PFILE 

OFILEsXFILE 

INITIALIZATION  FOR  VERTICAL  DAMPING 

H=IH+ALTERR 

HD’  ALTO 

CH3=  ALTO 

DELH=H8-H 

VDNP=CV03«QELH*0T 

NUP’0 

N0RTH=* 

WRITE  (0FILE.551)  DELH.VDMP 

COMPUTE  INITIAL  UEN-WA  TO  EARTH-FIXED  TRANSFORM,  A 

INAVLA=ILAT+<  LATERR 1PRDT0DG 
INAVLO=ILON*( LCNERR 1/RDT0D3 
SINLT’SINt INAVLA ) 

S3LAT=SINLT*3INLT 
SINL0N=3IN( IHAVLO ) 

COSLON’COSCNAVLO) 

COSLT’COSl INAVLA) 

ALF’WONDER 
CALF=COS(  WONDER) 

3ALF=SIN< WONDER) 

AU,1)=C0SLT*C0S10N 

A(2,l)=COSLT*SINLQN 

A<3,1)=SINLT 

A(  1,2  )=-CALr*iSINLON-SALFttSINLTMCOSLON 
A(2,2)=CCSL0N«CAt.F-SALF*SINlT*.3INl0N 
At  3,2)=  SALFt-COSLT 

A(  1 >3  )=  SALFfSIN1.0N-CALF«t!3INLTt*C0Sl0N 
A(  2 , 3)=-5AI.F*COSlON-CALF#SINLT*SINLQN 
At  3,3)=C0SLT*CALF 

WRITE  (0FILE.1225)  A 


COMPUTE  INITIAL  EARTH-RELATIVE  VELOCITY  IN  UEN-WA  FRAME 

DO  600  1=1,3 

NAVVt  X )=IVt  1 HVERRt  X ) 

600  CONTINUE 

Vt  1 IsNAVVt  3) 

V(2)=CAIF*NAW<1)  ♦ SAIF«NAVV<2) 

V(3)=-9AlF‘<NAVV(n  ♦ CALF*NAVV(  2 > 

XH=N*0.5»«VU>»DT 


00003610 
00003620 
00003630 
000Q36AQ 
00003650 
00003660 
00003670 
00003660 
00003660 
00003700 
00003710 
00003720 
00003730 
000037*0 
00003750 
00003750 
00003770 
00003780 
00003790 
00003600 
00003610 
00003020 
00003630 
000030*0 
00003850 
00003660 
00003670 
00003060 
00003890 
00003900 
00003910 
00003920 
00003930 
000039*0 
00003950 
00003960 
00003970 
00003900 
oor 035  90 
0000*000 
0000*010 
0000*020 
0000*030 
0000*0*0 
0000*050 
0000*0', 0 
0000*070 
0000*060 
0000*090 
0000*100 
0000*110 
0000*120 
0000*130 
0000*1*0 
0000*150 
0000*160 
0000*170 
0000*180 
0000*190 
0000*200 
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O O U O O U O U U O O U O U O t j !_>  uuuu  uuu  <0000  o oo 


ZERO  DVI 


40 


INITSW=1 

TLlNiT+DT 

RETURN 

" 551  FORMAT  <3t/3X,3F20.16),/) 

1000  FORHAT  tI5.F20.10) 

1010  FORMAT! 30H  NAVIGATION  INITIALIZATION 


00004210 
00004220 
00004230 
00004240 
00004250 
00004260 
00004270 
00004260 
00004290 
00004300 
00004310 
00004320 
00004330 
00004340 
00004359 
00004360 
00004370 
00004360 
00004390 
00004400 
00004410 
00004420 
00004430 
00004440 
00004450 
00004460 
00004470 
09004460 
00004490 
00004500 
00004510 
00004520 
00004530 
00004540 
00004550 
00004560 
00004570 
00004560 
00004590 
00004600 
00004610 
00004620 
00004630 
00004640 
00004650 
00004660 
00004670 
00004660 
00004690 
00004700 

H . ALTERS  ,NAW , VERR . ITEHP1 , LATEHH , ITEHP2 . LONERROOO  04  71 0 

00004720 
00004730 
00004740 
00004750 
0000476C 
00004770 
00004780 
00004790 
00004600 


C COMPUTE  INITIAL  ANGULAR  VELOCITY  OF  UEN-WA  FRAME  WRT  EARTH-FIXED 
C FRAME  IN  UEN-WA  FRAME 
RHO(1)=0.0 

CALL  AN6VE L( V . CALF . SA  LF » SSLAT . H > RHO ) 

WRITE  (0FILE.551)  RHO 

COMPUTE  COMP.  TO  INERTIAL  FRAME  RATE  X DT  AND  CORIOLIS  CORRECTIONS 

CALL  TORCOR! A.RHO. V»OT*THG»WXV) 

WRITE  (0FILE.551)  TKG 
WRITE  (0FILE«551)  WXV 

EXTRAPOLATE  A TO  MIO  COMP  CYCLE.  XA 

THY  = 0.5HRHQ! 2 )#QT 
THZ  a 0.5#RH01 3)#DT 
CALL  AUP(THY.THZ.M2DT) 

WRITE  (0FILE.551)  M20T 
CALL  MMfA.M2DT.XA) 

WRITE  fOFILE.551 ) XA 

EXTRAPOLATE  ALF  TO  MIO  COMP  CYCLE 

0ALF=( 0 . 5#V( 2 ) *SINLT«DT )/( RE«COSLT ) 

XSALF=SALF  ♦ DALF*CALF 
XCALF=CALF  - DALF#SALP 

COMPUTE  GRAVITY  AT  MIO  COMP  CYCLE 

CALL  GRAVIXA, XI). XSSLAT.GR ) 

WRITE  (QPILE.551)  GR 

COMPUTE  WXV  AT  MIO  COMP  CYCLE 

CALL  TORCOR t XA. RHO  > V » DT » TNG . WXV ) 


DO  640  J4al,3 
0V1( J4)=0.0 
CONTINUE 

INITIALIZATION  OUTPUT  AND  PRINT  CONTROL 

WRITE  (OFILE.IOIO)  OT , PRNTSW .OUTSW , OF I LE , SPARE 1 . PPNTOT 
X.CV01.CVD2.CVD3 


ITEMP1«ILAT«R0TC0G 

ITEMP2=ILON»ROTOOG 

URITE(OFILE.IOIS) 
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X/3X.8H 

OT 

>3X,G16.8,3X,4H  SEC. 

00004810 

X/3X.8H 

PRNTSW 

,3X,G16.S, 

00004820 

X/3X.8H 

OUTSM 

,3X,G16.8, 

00004830 

X/3X.8H 

OFILE 

.3X.I15. 

00F04840 

X/3X.8H 

SPARE 

,3X iG16.fi, 

00004650 

X/3X.8H 

PSNTOT 

, 3XiG16.fi, 

00004060 

X/3X.12H  CVDU5EC-1  )»3X,G16.8> 

00004370 

X/3X.12H  CV02!SEC-2),3X,616.8, 

00004880 

X/3X.12H  CV031SEC-3) ,3X,G16.6,//) 

00004890 

1012  FORMAT! 3X»30H 

INITIAL  VEHICLE  POSITION 

,/ 

00004900 

X 

6X.11H 

H(FT)  ,616.8./ 

00004910 

X 

6X.11H 

ALTERRt FT),G16.8»/ 

00004920 

X 

6X.11H 

VC  FT/SEC)  ,3616.8,/ 

00004930 

X 

6X.14H 

VERR! FT/SEC)  .3G16.8,/ 

00004940 

X 

6X.11H 

LAT(DEGS)  iG16.fi, / 

00004950 

X 

6X.13H 

LATERRIOEGS)  ,G16.8,/ 

00004960 

X 

6X.11H 

LON! DECS)  .G16.8,/ 

00004979 

X 

6X.13H 

LONERRtDEGS ), 616.8,//) 

00004980 

1200  F0RHAT(6X,25H 

LLN  **  H(FT) 

•G16.S,/ 

00004990 

X 

6X.2SH 

V! FT/SEC) 

>3G16.S,/ 

00005000 

X 

6X.25H 

LAT(OEGS) 

.G16.fi, / 

00005010 

X 

6X.25H 

LON! DESS) 

,G16.8>/ 

00095020 

X 

6X.25H 

PITCH! DECS) 

,616.8,/ 

00005030 

X 

6X.2SH 

ROLL!  DEGS) 

,616.8,/ 

00005040 

X 

6X.25H 

HEADING! DECS) 

•616.8,/) 

00005050 

1225  FORMAT! 6X.25H 

A TRANSPOSE 

,3(/3X,3616.8),/) 

00005060 

1235  FORMAT! 6X»25N 

OTEM 

,3</3X,3Q16.8),/) 

00005070 

1250  FORMAT! 55H  PITCH  IS  ♦ OR  -90OEGREES,  PREVIOUS  VALUES  ARE  OUTPUT  )Q0005080 
1300  FORMAT! 10X.3F20. 10,/)  00005090 

cm  oooosxoo 
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SUBROUTINE  ANGVEL  ( AVEL.ACALF.ASALF.ASSLiALT, 

1 RHO) 

COMPUTES  LEVEL  COMPONENTS  OF  ANGULAR  VELOCITY  OF  VEHICLE 
DUE  TO  ITS  NOTION  WRT  EARTH 

REAL  AVEL(  3),ACALF.ASALF,A5SL»ALT»VE»VN,WE»HN.RM,RP 
REAL  TMPI,RH0(3>,ESQ,RE3Q»RE,TNP2 
C 

DATA  ESS/6 .694317778E-3/ 

OATA  RESCUE. 078555712E7/ 

DATA  RE/C.0925640E7/ 

C 

VE  a ACALF*AVEL<2)  - ASALF**AVEL( 3) 

VN  a ASALF*AVEL(2)  ♦ ACAIF*AVEL(3) 

C 

THP1=1.Q  / (1.0  - ESQ»ASSL) 

TMP2-SQRT(  TNPI 1 
C 

RP  s ALT  4 RE»TMP2 

RM  = ALT  4 RE5Q»THP1*THP£ 

C 

RE  a -VN/RK 
UN  = VE/RP 
C 

RHO(I)  b 0.0 

RUOt 2 > a ACALF«WE  4 ASALFMMN 
RH0t3>  =-ASALF**WE  4 ACALFFWN 
. RETURN 
two 


OOOOOOIO 

00000020 

00000030 

00000040 

00000050 

00000060 

00000070 

00000080 

00000090 

OOOOOIGO 

ooooono 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

0000018Q 

00000190 

0O0QQ20C 

00900210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 

00000280 

00000290 

00000300 
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SUBROUTINE  TORCORl A.RHO.V.DT, 

1 THG>WXV) 

COMPUTES  ANGULAR  VELOCITIES  TIMES  DT  ANO  CORIOLIS  CORRECTIONS 

REAL  A{3,3),RH0t3),THGl3),WXV<3),THE!3).DT,WE,VC3),EAR{3) 

OATA  UE/7.292115147E-5/ 

00  1 J=l,3 

CALCULATE  EARTH  RATE  X OTIIN  COMP. ) 

EARIJ)  = A(3.J)»WE»0T 

CALCULATE  COMPUTATIONAL  FRAME  KRT  INERTIAL  FRAME  RATE  X DTI  FOR 

TKGIJ)  = EARIJ)  ♦ RH0IJ)»0T 

CALCULATE  RATE  X DT  (FOR  MXV» 

THE! J ) - THGIJ)  ♦ EARIJ) 

1 CONTINUE 

CALCULATE  CORIOLIS  CORRECTIONS  IFOR  VELOCITY  UPDATE) 

WXV(l)  = THSI3)*tV(2)  - THEI2)**VI3) 

UWt 2 ) = THEII)**VI3)  - THE! 3)*V( l ) 

UXVI3)  a TH212)*VI1)  - THEU>*VI2) 

RETURN 

END 


00000010 

00000020 

00000030 

00300040 

00000050 

00000060 

00090070 

ooooooeo 

00000090 

00000100 

00000110 

00000120 

00000130 

00000140 

000001  so 
QDIC1 100000160 
00000170 
00000100 
00000190 
00000200 
00003210 
00000220 
00000230 
00000240 
00000250 
00000260 
00000270 
00000200 
00000290 
00000300 
00000310 


I 
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SUBROUTINE  AUPfOY.OZ, 

1 HUP) 

C 

C FORM  SECOND  ORDER  UPDATE  MATRIX  FOR  IVWA  COMP. 
C TO  EARTH  FIXED  OCM 
C 

REAL  DY 
REAL  OZ 
REAL  HUP<3,3> 

C 

HUPU.U  = 1.0  - 0.5»(DY«OY*DZ*tDZ) 

HUPtE.l)  = DZ 
MUP(1»2)  = “15Z 
MUPU.SI  = OY 
MUPCSil)  s-DY 

HUPt  2»2 ) s 1.0  - 0.5*  DZ*DZ 
MUP(3,2)  = 0.5*0Y*DZ 
HUP1  2»3)  = MUP(3i2) 

HUP(3,3J  = 1.0  - 0.5*OY*DY 

RETURN 

END 


09000010 

Q0000020 

00000030 

00000040 

00000050 

00000060 

00000070 

ooooooao 

00000090 

00000100 

QOOOOUO 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

00000160 

00000190 

00000200 

00000210 


c 

c 

c 

c 

c 


c 


c 

c 

c 


SUBROUTINE  GSAV  (AM, AIT, 
1 $01,  G) 


COMPUTES  3 COMPONENTS  OF  GRAVITY  IN  COMPUTATIONAL.  FRAME 
USING  K3S-72  ELLIPSOIDAL  EARTH  MOOEL 


REAL  AMO,  3),  6(3),  SSL,  ALT,  CGEP 


SSL  = AH(3,1)*AH(3,1) 

COEF  = 1.63E-8#AIT#AM(  3,1) 


GUI  * -02.0877057  ♦ .16939081«SSL 

1 ♦ 7,5281E»<i»SSL#SSLHM1.0-(  9,62£7E«fi 

2 - 6.4089E-10*SSL)#4LT*6.8512E-15#ALT*ALT) 

G(2)  = COEF«AM( 3,2) 

6(3)  s CO£F*AMO>3) 


RETURN 

m 
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00000010 

00000020 

00000030 

00000040 

00000350 

00000060 

00000070 

C0C00030 

00000090 

000C01GQ 

ooooono 

00000120 

00000130 

00000140 

00000150 

00000160 

00000170 

00000180 

00000140 

00000200 

00000210 


o n o o 


SUBROUTINE  MM(A.B.C) 

MATRIX  MULTIPLY  SUBROUTINE 
t3X3)  X (3X3)  = (3X3)  J A«S=C 

REAL  A(3,3).BC3,3),C(3,3) 

DO  I 1=1,3 
00  1 Jn,3 

C(I,J)  =0.0 
DO  1 K=l,3 

C(I>J)  = C(I,J)  ♦ A(I,K)*BiK,J) 
1 CONTINUE 
RETURN 
END 


00000010 

00000020 

00000030 

00000040 

00000050 

00000060 

00000070 

00000060 

00000000 

00000100 

ooooono 

00000120 

00000130 

00000140 


o o n o o o 


00000010 


05/10/78  OATE  OF  CURRENT  MODULE 

00000020 

00000030 

THE  EVALUATION  MODULE  PRINTS  TABLES  OF  NAVIGATION  ERRORS  AND 

00000060 

TRAJECTORY  PARAMETERS. 

00000050 

00000080 

SUEROUTINE  EVU  T , IENDF , LAT , LON , ALT , VEL , DVT , PITCH  .ROLL . Y AW , 

00000070 

X 

MONO  ER , HAV  LAT , NAV  LON , NAVV » NAVI  i , NAVP , NAVR , NAVHD > 

00000000 

00000090 

FEAL 

ALT 

00000100 

REAL 

DATA(5) 

00000110 

REAL 

OT 

00000120 

PEAL 

OVT ( 3 ) 

00000130 

PEAL 

EALT(SO) 

00000160 

REAL 

ELATt  50 ) 

00000150 

-'EU 

ELONISOi 

00000160 

REAL 

. evelxiso; 

00000170 

REAL 

EVE  11s SO ) 

oooooieo 

REAL 

LVEL21  SO ) 

'00000190 

■ REAL 

LAT 

Q0C002O0 

. REAL 

LON 

0.0000210 

. REAL 

NAVLAT 

00000220 

■'EAL 

HAVLCN 

000002  30 

REAL 

HAVH  - 

00000260 

REAL 

NAVV131 

00000250 

peal 

PRMTOT 

00000260 

Rr  al 

POT 000 

00000270 

REAL 

' T ■ 

00000200 

CELL 

TEVL 

00000290 

P’.AL 

VEUJS 

00000300 

REAL 

XALTI50) 

03000310 

REAL 

XDVTXt  50  > 

00000320 

Real 

T.OVTYI  50 ) 

00000330 

REAL 

:ovtz(  so ! 

00000360 

peal 

XI.ATt  50 ) 

00000350 

REAL 

xlchiso) 

00P00360 

REAL 

XV ELM  501 

00000370 

REAL 

>.VtLY(  SO  1 

00000350 

REAL 

XVLUI50) 

00000390 

PEAL 

XTI50I 

00000600 

REAL 

XHS/0I60T 

00000610 

REAL 

XPtlCMt SOT 

00000620 

REAL 

XROltlSO* 

00300630 

REAL 

EHEAOISO) 

00000660 

PEAL 

EPITCHt  50  1 

00000650 

REAL 

EROILT50  1 

00000660 

00000670 

REAL 

NAVP 

00000600 

REAL 

NAVR 

00000690 

PEAL 

NAvHO 

00000500 

REAL 

PITCH 

0P9OO51O 

REAL 

ROLL 

00000520 

PEAL 

YMI 

00000530 

PEAL 

WONDER 

000005*9 

PEAL 

FGATAs  EOT 

00000550 

PEAL 

WE 

00000540 

REAL 

RE 

00900570 

real 

G 

00000500 

00000590 

ltvTCGtR  StUDF 

00000600 

l1- 90 


c 


c 


c 


c 

c 


c 


c 


c 


INTEGER  INITSW 
INTEGER  IFILE 
INTEGER  PFILE 
INTEGER  N 
INTEGER  OFILE 
INTEGER  OUTSW 
INTEGER  PPFILE 

EQUIVALENCE  (DATA(l),  DT) 
EQUIVALENCE  (DATA! 2),  PRNTSW) 
EQUIVALENCE  (0ATA<3),  OUTSW) 
EQUIVALENCE  (0ATA(9),  XFILE) 
EQUIVALENCE  (OATA(S)i  PRNTOT) 

EQUIVALENCE  (PDATAU),  WE) 
EQUIVALENCE  (FDATA12),  RE) 
EQUIVALENCE  (PDATA(3)»  G) 

OATA  ROTODG/57. 29577951/ 
DATA  INITSW/O/ 

OATA  IFILE/OO/ 

OATA  N/C/ 

DATA  PFILc/7/ 

OATA  PPF1LE/12/ 

IF  (IENOF.GT.O)  CO  TO  800 
IK  ( INITSW. EQ.O)  GO  TO  500 
IF  (T.LT.TEVL-.0005)  RETURN 


NmH*1 


00000610 

00000620 

00000630 

00000690 

00000650 

00000660 

00000670 

00000680 

00000690 

00000700 

00000710 

00000720 

00000730 

00000790 

00000750 

00000760 

00000770 

00000780 

00030790 

00000000 

00000610 

OOOUoOCO 

00000830 

00000890 

00000650 

00000660 

00000870 

00000300 

00000890 

00000900 

00000910 


XT(N)=T 

XLAT(N)5LAT*ROTOOG 
X LONi  M ) e LON»RDTODG 
XALT(NI=ALT 
XVELXtNlsVEU  1 ) 

XVELY(Ni=VEL<2) 

xveuumvcus) 

XOVTX(Nl:OVT(l) 

XOVTYf  N )»0VTI  2 ) 

XOVT?(N>*t>Vm> 

XHE  ADI  M ) a ( YAW-WGNOER ) *RD TOOG 
XPITCN<N)EPITCH*tPO'rcOG 
XROLU  M )3ROlL'lROTGDG 

EtAT(N):i LAT-NAVLAT l*RS 
ELOHUmt  LON-NAVLON)*»E#COSl  UT) 

EALT«N)«AtT-MAVH 
EVELX«M|sVEHl)  NAVVtl* 

E V6  L Y(  N ) - VE  L(  6 ) -NAVVl  2 ) 

£VELf(N)=VEU3)-NAVV(3) 

EHEAtHNlMTAW-WONOER-NAVNO  )»3600«ROTOOG 
ePITCH(N)e<PITCH*NAVPH3600t<RDTODG 
EROU  ( N )a  (ROU-HAVR  )*fJ600«RDT0OG 

WRITE  PLOT  FILE  C PPFILE I 

WRITE  ( PPFILE ) XT(N1  iXlAT<N),XLON(N)*XALTtN).XVElX)NI*XVELYtN) 
X XVEL2(NI,XHEAO(N),XPITCHtN),XROLL(M),EUTIN),ELOmN».EALT(NJ. 

X EVELX(N)  ■EV£LY(H)»EVEUXN)  >EHEAD(N)iEPITCHtN)iEKOLL(N) 


00,000920 

00000930 

00000990 

00000950 

00000960 

00000970 

00000900 

00000990 

00001003 

00001010 

00001020 

00001030 

00001090 

00001050 

00001060 

00001070 

00001080 

00001090 

OOQOUQO 

OOOOUIO 

00001120 

oooomo 

00001190 

00001150 

00001160 

*00001170 

00001100 

03001190 

00001200 
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IF  (N.LT.49.5)  GO  TO  900 
S00  CONTINUE 

OUTPUT  AMO  PRINT  CONTROL 

(A  TABLE  IS  PRINTED  EVERY  50  CYCLES) 

LAST  PASS  COMES  HERE  TO  PRINT  ANY  REMAINING  DATA 

IP  (PRNTSW.LT. I.)  GO  TO  999 

WRITE  (OFILEilCOO) 

WRITE  (OFILE, 1300) 

WRITE  ( )FIL£  .1320) 

00  SIO  1=1. N 

WRITE  (OFILE , 1400  )XT(I ) ,XLAT(  1 1 ,XLON<  X ) ,XALTl I ) ,XVELX( I 
X XVELZf I )>XNEAO(X ) .XP1TCHC  X ) ,XRQLL< X ) 

810  CONTINUE 

WRITE  (OFILE.1CSO) 

WRITE  (OFILE, 1300) 

WRITE  (OFILE. 1330) 

CO  800  1=1. N 

WPITE  (OFILE.  1400  )XT(I  I, ELAT(  I ) ,ELON(  X ),EALT( X ),EVEU( X 
X EVELZ( X ). ENEAD( X ) ,EPXTCH( X ) ,EROLL( X ) 

SCO  CONTINUE 


900  CONTINUE 

999  CONTINUE 
Tt*VL=T«DT 
RETURN 


EVALUATION  HOOULC  INITIALIZATION 

500  CONTINUE 
REWIND  IfILE 
ROUIND  PFILE 

501  READ  ( IFILE .1000)  IX.OATAUX) 
IF  (EOmPXtE))  502*501 

502  CONTINUE 
REWIND  IFILE 

503  READ  (PFILE* 10001  IV.POATAIXX) 
IF  I EOF! PFILE))  510*503 

310  CONTINUE 
REWIND  PFILE 


INITIALIZATION  OUTPUT  AND  PRINT  CONTROL 
OFXLE»Xme 

WRITE  IOFILC.1100)  OT*PflNI5W,OUT5W.OFXLEiPSNTOr 


INIT5W=1 


00001210 
60001220 
00001230 
00001C40 
00001250 
00001260 
00001270 
000012C0 
00001290 
00001300 
00001310 
00001320 
00001330 
00001340 
00001350 
)>XVELY(  I) *00001360 
00001370 
00001380 
00001390 
00001400 
00001410 
00001420 
00001430 
00001440 
)*EVELT(X) *00001450 
00001460 
00001470 
0000)480 
00001490 
00001500 
00001510 
00001520 
00001530 
00001540 
00001550 
00001560 
00001570 
OOPOISCO 
00001S90 
00001600 
00001610 
00001620 
00001630 
00001640 
00001650 
00001660 
00001670 
00001680 
00001690 
00001700 
00001710 
00001720 
0000l7i0 
00001740 
0000)730 
00001760 
00001770 
00001780 
00601790 
00001800 


1 1 

si! 
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TEVL=T*OT 

RETURN 

c 

1000  FORMAT) 15 « F20 . 10 ) 


1100  FORMAT) 30H 
X/3X.0H  OT 
X/3X.0H  PfiNTSW 
X/3X.8H  OUTSU 
X/3X.GH  CFILE 
X/3X.0H  PRMTOT 


EVALUATION  INITIALIZATION 
>3X,G16.6,3X,4H  SEC, 
,3X,G16.8, 

>3X,G16.6, 

,115, 

, 3X.G16.6,//) 


1200  FORMAT ( INI , J6X » 11H  TRAJECTORY) 

*1250  FORMAT) 1H1,36X,18H  NAVIGATION  ERRORS) 


1400  FORMAT!  F9.  J.1X,9(G12.5»1X) ) 

u 

1500  FORMAT) 10X.II5) 

DC- 


00001010 
00001020 
00001030 
00001040 
00001050 
00001060 
00001070 
00001060 
00001690 
00001900 
00001*' 0 
00001920 
00001930 
00001940 
00001950 
00001960 


1300  FORMAT) 2X.5H  TIME,3X,0H 

IAT.5X.0H 

LON.5X.0H 

ALT ,5X, 

00001970 

X 

9N  VXt  EAST ) ,4X,10H  VYINORTK ) ,3X,6H 

VZ(UP),5X.6H 

HEA0.5X, 

00001900 

X 

0H 

PITCH, 5X.6H 

ROLL) 

00001990 

00002000 

1320  FORMAT) 8H 

( SEC  > >2X,6H 

(0EG),5X,6H 

(DEG1.5X.0H 

(FT),5X, 

00002010 

X 

OH 

)FPS)«5X,0H 

) FPS ) ,5X,6H 

(FPS),5X,6H 

<0EG),5X, 

00002020 

X 

» 

6H 

(DEG),5X,0H 

(DEG)./) 

00002030 

00002040 

1330  FORMAT) 0H 

1 SEC  > ,2X,6H 

)FT),5X,flH 

(FT),5X,0H 

(FT),5X, 

00002050 

X 

eH 

)FPS),5X,6H 

( FPS),5X,0H 

(FPS),5X,6H 

(SEC).SX, 

00002060 

X 

OH 

) SEC ) ,5X,9H 

(SEC),/) 

00002070 

00002090 

00002090 

00002100 

00002110 

00002120 
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FUNCTION  GAUSS 

THIS  FUNCTION  CALCULATES  A GAUSSIAN  RANDOM  NUMBER 

IT  IS  MACHINE  INOEPENOENT  AND  WILL  WORK  PROPERLY  IF  THE  SINGLE 

PRECISION  UORO  OF  THE  MACHINE  IS  MORE  THAN  20  SITS  LONG 


FUNCTION  GAUSS  (MEAN  ,ST01 
REAL  NEANtSTO 
OATA  1/0/ 

IrtI.Eq.il  GOTO  10 
IX  = 3 
1=1 
10  CONTINUE 
FA  = 0. 

DO  40  J e 1.12 

IY  a IX  * 1024  ♦110795 

IY  = MOOtlY, 5242001 

FY  = IY 

FA  e FY  h 1.907J49E-6  ♦ FA 
IX  a IY 
40  CONTINUE 

GAUSS  * MEAN  ♦ STO«(FA  -0.1 
RETURN 
END 
C 
C 


OOQQQOIO 

00000020 

00000030 

00000040 

00000050 

00000060 

00000070 

00000030 

00000090 

00000100 

00000110 

00000120 

00000130 

00000140 

00000150 

00000160 

03000170 

QQOOOIGO 

00000190 

00000200 

00000210 

00000220 

00000230 

00000240 

00000250 

00000260 

00000270 


SUBROUTINE  HTXM(MA»MB,MC) 
REALM#  MA<9), 118(91. MC<9> 
REALM#  VIN(3),V(3> 

00  10  1=1.3 
VINtl )=NB( I ) 

VINt  2 )=MB( 1+3) 

VINt  3)*HB( 1*6 1 
CALL  MTXV  (MA.VIN.V) 

MCI  1 1 = VC  1 J 
t1C(I*3)  = Vt  2 1 
MCI  1*6 ) = V(3) 

10  CONTINUE 
RETURN 
ENO 


00000010 

00000020 

00000030 

00000090 

00000050 

00000060 

00000070 

00000060 

00000090 

00000100 

00000110 

00000120 

00000130 

00000190 

00000150 


SUBROUTINE  MTXV(M.V.nV) 

REALMS  M(9)»V(3),MV(3) 

MV( X ) - N(  1 )»V(  1 ) ♦ M(4)»V(2)  ♦ M(7»*V(3> 
NVI 2 ) = H(2)«V<1)  ♦ H(5)«V(2)  ♦ M(61»V(3) 
MV( 3 ) = H(S)*VU>  ♦ M6)*VJ2>  ♦ H(9>*V(3> 
RETURN 
EK3 


00000010 

00000020 

00000030 

00000040 

00000050 

00000060 

00000070 

OOOOOOBO 


1-96 


u 


SUBROUTINE  MXMf MA.HB.MAB) 

00000010 

REAUS  MAI  9)iM3(  9),MAB(  9) 

00000020 

CALL  MTXV  < MB(MA( 1 ) (MABJ 1 ) ) 

00000030 

CALL  MTXV  <MB,MA<4),MAB(4>) 

00000040 

CALL  MTXV  (MB»MA( 7) »MAB( 7) ) 

00000050 

RETURN 

00000060 

END 

00000070 

OOOOOQQO 

1-97 


SU3R0UTINE  HXV(H,V1,V2> 

REALMS  Ml  9 ) , VXt J),V2C3) 

V2tl»  * MIllWMlt  ♦ Ml2)»Vlt2>  ♦ M(J»«V1(5) 
V212)  s I1(4I«V1(1)  ♦ Ml 5)*Vit 2 ) ♦ M(6)*V1IJ) 
V2t 3 ) = M7HVXU)  ♦ H(6>*V112>  ♦ H19)*»V113) 
RETURN 
ENO 


00000010 

00000020 

00000030 

00000040 

OOOOOOEO 

00000060 

00000070 

coooooao 
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